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Abstract 

This  paper  studies  control  problems  of  sampled  data  systems  which  are  subject  to  random  sample 
rate  variations  and  delays.  Due  to  the  rapid  growth  of  the  use  of  computers  more  and  more  systems  are 
controlled  digitally.  Complex  systems  such  as  space  telerobotic  systems  require  the  integration  of  a  number 
of  sub-systems  at  different  hierarchical  levels.  While  many  sub-systems  may  run  on  a  single  processor,  some 
sub-systems  require  their  own  processor  or  processors.  The  sub-systems  are  integrated  into  functioning 
systems  through  communications.  Communication  between  processes  sharing  a  single  processor  are  also 
subject  to  random  delays  due  to  memory  management  and  interrupt  latency.  Communications  between 
processors  involve  random  delays  due  to  network  access  and  to  data  collisions.  Furthermore,  all  control 
processes  involve  delays  due  to  causal  factors  in  measuring  devices  and  to  signal  processing. 

Traditionally,  sampling  rates  are  chosen  to  meet  the  worst  case  communication  delay.  Such  a  strategy 
is  wasteful  as  the  processors  are  then  idle  a  great  proportion  of  the  time;  sample  rates  are  not  as  high 
as  possible  resulting  in  poor  performance  or  in  the  over  specification  of  control  processors;  there  is  the 
possibility  of  missing  data  no  matter  how  low  the  sample  rate  is  picked. 

Randomly  sampled  systems  have  been  studied  since  later  1950’s,  however,  results  on  this  subject  are 
very  limited  and  they  are  not  applicable  to  practical  systems.  This  paper  studies  asymptotical  stability 
with  probability  one  for  randomly  sampled  multi-dimensional  linear  systems.  A  sufficient  condition  for 
the  stability  is  obtained.  This  condition  is  so  simple  that  it  can  be  applied  to  practical  systems.  A  design 
procedure  is  also  shown. 

These  results  are  applied  to  robot  control  systems  using  PD  controllers  with  a  feedforward  term,  com¬ 
puted  torque  controllers  or  simple  computed  torque  controllers.  The  effectiveness  of  the  method  is  demon¬ 
strated  by  simulations. 
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Abstract 


This  paper  studies  control  problems  of  sampled  data  systems  which  are  subject  to  random  sample 
rate  variations  and  delays.  Due  to  the  rapid  growth  of  the  use  of  computers  more  and  more  systems  are 
controlled  digitally.  Complex  systems  such  as  space  telerobotic  systems  require  the  integration  of  a  number 
of  sub-systems  at  different  hierarchical  levels.  While  many  sub-systems  may  run  on  a  single  processor,  some 
sub-systems  require  their  own  processor  or  processors.  The  sub-systems  are  integrated  into  functioning 
systems  through  communications.  Communication  between  processes  sharing  a  single  processor  are  also 
subject  to  random  delays  due  to  memory  management  and  interrupt  latency.  Communications  between 
processors  involve  random  delays  due  to  network  access  and  to  data  collisions.  Furthermore,  all  control 
processes  involve  delays  due  to  causal  factors  in  measuring  devices  and  to  signed  processing. 

Traditionally,  sampling  rates  are  chosen  to  meet  the  worst  case  communication  delay.  Such  a  strategy 
is  wasteful  as  the  processors  are  then  idle  a  great  proportion  of  the  time;  sample  rates  are  not  as  high 
as  possible  resulting  in  poor  performance  or  in  the  over  specification  of  control  processors;  there  is  the 
possibility  of  missing  data  no  matter  how  low  the  sample  rate  is  picked. vK '  „ r  r  i<  ' 

Randomly  sampled  systems  have  been  studied  since. later  1950’s,  however,  results  on  this  subject  are 
very  limited  and  they  are  not  applicable  to  practical  systems.  This  papeY  studies  asymptotical  stability 
with  probability  one  for  randomly  sampled  multi-dimensional  linear  systems.  A  sufficient  condition  for 
the  stability  is  obtained.  This  condition  is  so  simple  that  it  can  be  applied  tto  practical  systems.  A  design 
procedure  is  also  shown.  ~  o.-,  ><.  ,  N 

These  results  are  applied  to^obot  control  systems  using  PD  controllers  wjilh  a  feedforward  term,  com¬ 
puted  torque  controllers  or  simple  computed  torque  controllers^  The^effectTveness  of  the  method  is  demon¬ 
strated  by  simulations.  - 
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Chapter  1 

Introduction 


Robots  today  have  developed  beyond  the  single  processor  controlled  manipulator  whose  only  interaction 
with  the  outside  world  is  by  means  of  a  small  number  of  binary  inputs  and  outputs.  Robots  now  comprise 
planning  and  execution  systems,  multiple  manipulators,  stereo  vision  systems,  dense  range  finders,  tactile 
sensors,  etc.  These  sensors  and  actuators  are  typically  realized  in  terms  of  single  or  multiple  processor 
systems  and  their  integration  into  functioning  robotic  systems  is  a  subject  of  current  research  in  a  number 
of  laboratories.  Any  form  of  such  integration  must  be  based  on  communications,  both  for  sending  control 
sequences  and  collecting  measurement  data.  While  the  use  of  large  virtual  memories  have  been  proposed 
as  a  solution,  this  form  of  communication  is  undesirable  due  to  hardware  conformity,  timing  and  loading 
constraints,  lack  of  communications  control,  the  need  to  link  many  modules  into  a  single  monolithic  system, 
etc.  A  more  attractive  communication  scheme  is  based  on  some  form  of  local  area  network  in. which 
hardware  conformity  is  minimized,  timing  constraints  are  virtually  eliminated,  communications  may  be 
monitored  and  controlled,  individual  systems  may  be  developed  and  modified  separately,  etc.  Another 
important  consideration  is  the  replacement  of  a  multi-conductor  bus,  with  severe  timing  and  loading 
constraints,  with  a  network  requiring  only  a  single  conductor  which  may  even  be  disconnected  while  the 
system  is  running. 

While  all  control  processes  involve  delay,  causal  delay  in  measurement  devices,  signal  processing,  etc., 
multi-process  computer  systems  involve  additional  variable  delays:  interrupt  latency,  priority  scheduling, 
conditional  branching,  etc.  Local  area  networks  complicate  the  delay  problem  with  addition  delays  due 
to  network  access  and  collisions.  These  delays  affect  system  performance  and  stability.  Designs  for  such 
systems  must  be  based  on  a  control  theory  which  includes  not  only  delay  elements  but  elements  which  have 
delays  with  a  statistical  distribution. 

A  sampled-data  system  in  which  the  sample '.rate  is  random  is  Galled  a  randomly  sampled  system.  The 
papers  dealing  with  randomly  sampled  control  systems  are  not  so  many.  The  subject  was  first  addressed 
by  Kalman  (1957)  [10].  In  his  paper,  bounded-input  bounded-output  stability  problems  were  discussed 
for  randomly  sampled  systems  in  terms  of  the  second  moment  of  the  output.  Kalman  (1962)  [11]  later 
discussed  optimal  regulation  problems  of  randomly  varying  discrete  time  linear  systems  which  include 
randomly  sampled  systems.  Leneman  (1968)  [15]  studied  a  single-input  single-output  randomly  sampled 
first  order  systems  and  derived  a  condition  for  the  second  moment  stability.  Kushner  and  Tobias  (1969) 
[14]  studied  an  autonomous  linear  system  with  linear  or  nonlinear  feedback.  Using  a  stochastic  Ljapunov 
function,  they  obtained  conditions  for  the  stability  with  probability  one  and  the  s-th  moment  stability  for 
one  dimensional  systems.  They  also  derived  a  condition  which  is  sufficient  for  the  stability  with  probability 
one  and  is  necessary  and  sufficient  for  the  second  moment  stability  for  multi-dimensional  systems.  Assuming 
statistical  independence  among  the  sampling  rates  and  signals,  Dannenberg  and  Melsa  (1975)  [5]  obtained 
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the  equation  of  the  expectation  of  the  state  and  the  output  and  investigated  the  stability  in  the  first  moment. 
In  1982,  Koning  [12]  studied  infinite  horizon  optimal  control  problems  of  linear  discrete  time  systems  with 
stochastic  parameters.  He  showed  that  if  the  system  is  second  moment  stabilizable  and  second  moment 
observable,  then  the  optimal  problem  has  a  unique  solution  and  the  closed  system  is  second  moment  stable. 
He  applied  this  result  to  stationary  optimal  control  problems  of  randomly  sampled  control  systems  with 
long-term  average  integral  criteria  and  investigated  the  influence  of  random  sampling  on  the  criterion  value 
by  means  of  simple  examples  [13].  He  pointed  out  that  random  sampling  may  increase  or  decrease  the 
stability. 

Randomness  of  sampling  operation  occurs  when: 

(a)  measurements  are  interrupted  randomly  by  others  [7]; 

(b)  the  system  does  not  use  a  constant  sampling  interval  and  executes  routine  tasks  such  as  measuring, 

calculation,  output  and  pre-calculation  repeatedly  without  any  waiting  period; 

(c)  the  controllers  are  interrupted  randomly  by  other  controllers  or  have  to  wait  to  access  global  memory 

in  the  network. 

For  case  (a),  we  can  apply  a  Bernoulli  distribution  for  the  probability  of  the  failure  of  the  measurement 
at  each  sampling  rate  [7].  For  case  (b),  the  randomness  is  due  to  conditional  branches,  mainly  contained 
in  the  program,  and  it  is  subject  to  a  normal  distribution  with  a  small  variance  or  to  a  shifted  waiting 
distribution1  with  a  small  average  arrival  time.  For  case  (c),  there  are  two  phases  and  a  combination  of 
two  distributions  may  be  reasonable.  In  a  usual  operating  situation,  the  sampling  interval  may  be  subject 
to  a  very  narrow  distributions  such  as  case  (b)  but  sometimes  the  controller  may  have  to  wait  for  a  rather 
long  time  compared  to  the  other  case  when  controllers  must  exchange  their  information,  access  to  global 
memory,  and  so  on.  Then  the  sampling  intervals  would  be  subject  to  a  normal  distribution  with  a  small 
variance  with  probability  e  and  a  shifted  waiting  distribution  with  a  rather  large  average  arrival  time  'With 
probability  1  —  c,  where  £  is  called  the  hit  probability. 

This  paper  studies  the  stability  of  randomly  sampled  systems  in  relation  to  the  random  sampling 
processes  and  addresses  the  communication  network  requirement  in  terms  of  system  control  performance. 
Though  Kalman  [10],  Kushner  ei  a/.  [14]  and  Koning  [12]  have  obtained  necessary  and  sufficient  conditions 
for  the  stabiUty  in  the  second  moment,  it  is  not  so  easy  to  apply  these  conditions  to  practical  systems. 
This  paper  studies  asymptotic  stability  with  probability  one  and  gives  a  necessary  and  sufficient  condition 
for  one-dimensional  systems  and  a  sufficient  condition  for  multi-dimensional  systems.  These  conditions  are 
easy  to  verify  for  given  sampling  distributions  and  are  thus  applicable  to  practical  systems. 

It  is  clear  from  the  study  of  randomly  sampled  systems  that  the  distribution  of  random  sampling 
processes,  the  characteristic  of  communication  networks,  has  a  decisive  influence  on  the  stability  and 
performance  of  robotic  systems.  In  addition,  the  results  of  this  paper  provide  a  theoretical  basis  for 
designing  communication  networks  for  hierarchically  distributed  control  systems  such  as  the  NASA/NBS 
Telerobot  Control  System  Architecture  [1]. 

In  the  next  chapter,  we  discuss  the  stability  problem  for  linear  time  invariant  systems.  First  of  all,  the 
asymptotical  stability  with  probability  one  is  defined  and  a  necessary  and  sufficient  condition  is  given  for 
one-dimensional  linear  time- invariant  randomly  sampled  systems.  Next,  the  results  are  extended  to  multi¬ 
dimensional  linear  time-invariant  randomly  sampled  systems  and  a  sufficient  condition  is  also  obtained. 
In  Chapter  3,  the  stability  of  a  randomly  sampled  robot  control  system  is  considered.  By  linerizing 
the  system  equation  and  the  controller  equation  along  the  desired  trajectory,  linear  time-variant  randomly 
sampled  systems  are  derived  and  the  stability  is  discussed  under  a  PD  controller  with  a  feedforward  term,  a 

'here  a  shifted  u /ailing  distribution  means  that  the  sampling  interval  =  constant  interval  -|-  a  random  variable  which  is 
subject  to  a  waiting  distribution. 
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computed  torque  controller,  and  a  simple  computed  torque  controller,  respectively.  In  Chater  4,  the  results 
given  in  Chater  3  are  applied  to  PUMA  260.  First  of  ail,  the  linerlized  dynamic  equation  is  obtained  by  a 
Lisp  program.  Next,  the  stability  is  discussed  for  the  cases  of  a  computed  torque  controller  and  a  simple 
computed  torque  controller.  Conclusions  are  given  in  Chapter  5. 
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Chapter  2 

Linear  Time  Invariant  Control 
Systems 


In  this  section,  we  define  the  asymptotically  stability  with  probability  one  for  a  linear  randomly  sampled 
controls  systems,  and  obtain  a  necessary  and  sufficient  condition  for  one-dimensional  systems.  A  sufficient 
condition  is  given  for  multi-dimensional  systems  and  a  design  procedure  of  feedback  gains  is  also  discussed. 

2.1  Definition  of  Stability 

Consider  a  linear  time-invariant  control  system 

■x(t)  =  Ax(t)  +  Bu(t)  (2.1) 

where  x  is  an  n-dimensional  state  vector,  u  an  r-dimensional  control  vector,  and  A  and  B  are  n  x  n  and 
n  x  r  matrices,  respectively.  For  this  system,  we  apply  a  constant  state  feedback  input 

u(t)  =  Kx(tk)  (2.2) 

from  t  =  t).  to  t  —  <fc+i(=  4  +  At),  where  K  is  an  r  x  n  matrix.  Then  x(4+i)  is  given  as  follows. 

*(t*+i)  =  (*(*0  +  n*k)K)x(tt)  (2.3) 


where 

and 


$(At)  =  exp(AAfc) 


«(A 


/■a* 

:)=  exp(.' 
JO 


Ar)drB. 


Sampling  interval  A*  is  assumed  to  be  subject  to  a  same  distribution  for  all  k  and,  A,  and  A;(i  ^  j) 
are  statistically  independent  of  each  other.  For  simplicity,  we  write  Eq.  (2.3)  as  follows 


*i+i  =  T(Ai)xi. 

The  stability  of  a  randomly  sampled  control  system  Eq.  (2.4)  is  defined  as  follows. 


(2.4) 
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Definition  1  (Stability)  The  randomly  sampled  system  Eq.  (2-4)  i s  asymptotically  stable  if  for  any 
positive  €  and  p,  there  exits  some  integer  N  such  that 

ProifllzjtH  >  c\  <  p,  for  k  >  N 


for  any  xo,  or  alternatively 


lim  Xk  =  0, 
k— co 


with  probability  one  for  any  zq. 


2.2  One-Dimensional  System 

In  this  section,  the  asymptotic  stability  defined  above  is  discussed  for  one-dimensional  systems 

x  (t)  =  Ai(<)  +  Bu(t) 

where  n  =  1  and  r  =  1.  We  define  following  notations: 

£(w]  :  expectation  of  random  variable  w 

Vfw]  :  variance  of  random  variable  cj 

7Pog(|r(A)|)l  <  oo 


(2.5) 


and  assume  that 

and 


£7[(log(|r(A)|))3]  <  co 

The  following  theorem  is  used  in  the  proof  of  the  next  proposition. 


(2.6) 

(2.7) 


Theorem  1  (Berry-Esseen  Theorem)  [18]  Consider  a  sequence  x\,X2,  -  ■  •  of  independent  random  vari¬ 
ables  such  that 

£(*.•]  =  0,  E[xf]  =  erf ,  £(*•]  <  ccr,2. 

We  form  the  sum 


=  -!>■’  <r2  =  E<r>?- 


Then  the  distribution  of  x  converses  to  the  normal  distribution  in  the  following  sense: 

\f(x)~g(x)\<  , 

<T 

where  f(x)  and  g(z)  are  the  distribution  functions  of  x  and  the  normal  random  variable  z  with  zero  mean 
and  unit  variance. 

Then  the  following  proposition  holds. 

Proposition  1  (One-Dimensional  System)  Under  Assumptions  (2.6)  and  (2.7),  one-d tmensional  ran¬ 
domly  sampled  system  (2.5)  is  asymptotically  stable  with  probability  one  if  and  only  if 


£[log(|r(A)|)]  <  0. 
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<  proof  > 

Assuming  xo  ^  0  without  loss  of  generality,  from  Eq.  (2.4)  we  have 

t-l 

log(l**l/l*o|)  =  2ZIo*(lr(A<)l)-  (2.8) 

i=0 

From  the  statistical  independence  of  A,’s  and  Thebysheff’s  inequality  [19],  the  following  equation  is  de¬ 
duced. 

P^(|M'*tt/r0l)  -E\>c\<^  (2.9) 

for  any  s  and  any  it,  where  E  and  V  stand  for  £'[log(|r(A)[)]  and  l^[log(|r(A)|)]  respectively. 

(i)  Sufficiency.  Now  suppose  that  E  <  0  and  set  t  equal  to  —E/2.  Then  we  have 

log(|zj=/zo|)  >  — >b£"/2 ,  for  any  k  with  probability  less  that  or  equal  to 

This  shows  that  for  any  positive  numbers  e  and  p, 

Pro&[|zi|  >  e]  <  p,  for  k  >  max{-§  log(j^j),  £~-} 

or 

lim  xt  =  0,  with  probability  one. 

k— co 

(ii)  Necessity.  For  the  case  of  E  >  0,  if  we  use  £  =  E/2,  then  we  have 

logflzfc/zol)  >  kE/2,  for  any  k  with  probability  larger  than  1  - 

This  means  that 

i 

•  •  lim  \xi  |  =  co,  •  with  probability  one. 

k — oo 

For  the  case  of  E  =  0,  from  Theorem  1,  Assumptions  (2.6),  and  (2.7),  we  have 

|/(x)  -S(z)|  <  4- 

< 7 

where 

^lorife/sol),  ,  =  JkV 

a 

Therefore  we  have 

Pro6(|z0|  <  |zt|  <  |zo|exp{\/F7}]  =  Pro6(0  <  x  <  1]  >  f 

Jo 

Since 

g(x)dx  =  0.34134 

if  we  use  £  =  |zo|  and  p  =  0.3,  then  we  can  find  out  k  >  N  for  any  N  such  that 

Pro6[|zi|  >  £}  >  p 
a 

Note  that  Assumption  (2.7)  was  used  ony  in  the  case  of  E  =  0  of  Necessity  part. 


g(x)dx  - 


4c 

VkV 
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Now  defining 

7(A)  ■=  log(l|r(A)||)  (2.10) 

9(  A)  =  /  7  {r)dr  (2.11) 

Jo 

then  we  have  the  following  corollary. 

Corollary  1  (Bernoulli,  Uniform,  and  Mixed  Uniform  Distributions) 

i.  If  the  sampling  rate  A  is  subject  to  a  Bernoulli  distribution  where  A  =  a  with  probability  p  and  A  =  8 

with  probability  q  =  1—  p,  then  the  one-dimensional  randomly  sampled  system  (2.5)  is  asymptotically 
stable  with  probability  one ,  if  and  only  if 

P7(or)  +  q-y(0)  <  0. 

ii.  If  the  sampling  rate  A  is  subject  to  a  unifonn  distribution  U[a,  0\,  then  the  system  is  asymptotically 

stable  with  probability  one,  if 

9(0)  ~  9(a)  <  0. 

iii.  If  the  sampling  rate  A  is  subject  to  U[a,0\  with  probability  c  and  to  U[ 7,  <5]  with  probability  1  —  -  ,  then 

the  system  is  asymptotically  stable  with  probability  one,  if 

m- n(°)  +  {l _ e)Sm- <,h)  K „ 

(3  —  a  0—7 

■  <  proof  > 


i.  For  the  Bernoulli  distribution,  the  expectation  is  given  as  follows. 

£P°g(|r(A)|)]  =  P7(ar)  +  q(3. 

Assumptions  (2.6)  and  (2.7)  are  not  satisfied  only  if  T(a)  =  0  or  F(/?)  =  0.  Here  we  assume  that 
r(o)  =  0.'  In  this  case,  we  have  always  £’pog(jr(A)|)]  <  0  and 

Prob[\xk\  #  0]  =  qk,  for  any  ro  ^  0. 

Hence  the  system  is  asymptotically  stable,  and  the  condition  is  necessary  and  sufficient. 

ii.  Next  we  consider  of  the  uniform  distribution.  For  this  case,  the  expectation  is  given  by  the  following 

equation. 

gpog(ir(A)|)]  = 

p  —  a 

Therefore  the  condition  in  the  corollary  implies  that  the  expectation  is  negative. 

From  Eq.  (2.3),  we  have 

r(A)  =  expjAA)  +  ^p(exp{AA}  —  1). 

A 

Therefore  log(|r(A)|)  is  upper  bounded  for  the  uniform  distribution.  If  T(A)  ^  0  for  all  A  €  [a,/?], 
then  the  logarithm  is  also  lower  bounded  and  Assumptions  (2.6)  and  (2.7)  are  satisfied.  Hence  the 
corollary  is  true. 
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Now  we  assume  that  r(A)  =  0  for  some  A  €  [a,/?].  Since  T(A)  is  a  monotone  function,  it  has  only 
one  root  A*  at  most.  We  approximate  T( A)  by  the  following  A)  for  a  small  positive  number  £. 


ff(A)  = 

where 

At 

A2 

and  consider  a  autonomous  random  coefficient  system: 

2fc+l  =  ff(Afc)zi,  zq  =  xq- 

Then  we  can  find  out  some  £  >  0  such  that  £[log(f j(A))]  <  0,  because  of  the  continuity  of  T(A). 
Assumptions  (2.6)  and  (2.7)  are  satisfied  for  this  £  and  the  autonomous  system  is  asymptotically 
stable  with  probability  one.  At  the  same  time,  we  have 

|xfc!  <  |z*|,  for  any  fc  =  0,1,2,  ••  •. 

Therefore  the  one-dimensional  system  is  also  asymptotically  stable  with  probability  one. 
ill.  For  the  mixed  uniform  distribution,  the  proof  is  trivial  from  the  above  proof  for  the  uniform  distribution. 
□ 

Example  1  (Bernoulli  Distribution)  We  consider  the  stability  of  the  following  simple  system: 

x  =  u  (2.12) 

u  =  -1.  (2.13) 

Then  T  is  given  as  follows : 

r  =  1  -  a. 

■  First  of  all,  we  assume  that  the  sampling  interval  A  subjects  to  a  Bernoulli  probability  distribution  as 
follows; 

a  with  probability  p 
B  ■  with  probalility  q  » 

where  q  =  1  —  p.  Then  the  stability  condition  is 

£Pog(r>]  =  plogfll  -  o|)  +  ?log(|l  —  /?|)  <  0 
or 

|1  —  at|p  1 1  —  BV  <  1-  (2.14) 


r  |r(A)|  A€[a,Ai]U  [A2l/3] 
\  |r(A,)|  A  €  [Ai ,  A2] 

=  A*  —  £ 

=  A*  -  4  log (2  -  exp{A£}) 


Remarks: 

•  Eq.  (2.14)  gives  intuitive  understanding  of  the  proposition.  Namely,  for  large  k,  we  can  expect  k  x  p 
times  occurrences  of  A,-  =  a  and  k  x  q  times  of  A,-  =  B •  Therefore  Eq.  (2.14)  is  a  necessary  and 
sufficient  condition  for  the  asymptotic  stability. 
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•  Kushner  and  Tobias  [14]  obtained  a  sufficient  condition  for  £[|z(&)|’]  — 1 ►  0  and  |x(i*)|  — »  0  with 
probability  one  as  follows: 

£fll-A|']<l.  (2.15) 

This  equation  implies  if  —0.5  <  0  <  2.5  for  a  —  p  =  q  =  0.5,  then  |x(k)|  — »  0  with  probability  one. 
On  the  other  hand,  Eq.  (2.14)  obtains  — 1  <  /3  <  3  for  the  same  situation. 

Example  2  (Uniform  Distribution)  Next  we  assume  that  A  subjects  to  a  uniform  probability  distribu¬ 
tion  U[a,  0].  The  probability  density  function  is  given  as  follows: 

f(  =  »/Ae[a,/3], 

'  '  (0  :/ A  <  a  or  A  >  0. 

We  define  a  function  #(A)  as  follows: 

9(  A)=  /0Alog(|l-r|)dr 

=  (A  —  1)  log(|A  —  1 1)  —  A,  (2.16) 

then  the  asymptotic  stability  condition  is  given  as  follows: 

£(log(|l  -  A|)]  =  9ijZ9+-  <  0  (2.17) 

or 

(2.18) 

The  graph  of  g( A)  is  shown  in  Fig.  2.1.  From  tkis  graph,  we  can  obtain  the  maximum  3  for  given 
a  that  makes  the  system  stable.  For  example,  if  a  =  0.5  then  0  must  be  less  than  3.86  to  maintain  the 
stability.  Fig.  2.2  shows  a  simulation  forU[ 0.5,  3.5]. 

Remarks: 


•  T  is  stable  if  and  only  if  A  <  2  in  usual  sense.  Therefore  the  unstability  for  A  6  (2, 3.5]  is  compensated 
by  the  stability  for  A  €  (0.5,  2]. 

•  There  are  upper  bounds  for  both  a  and  0,  respectively.  Namely,  if  0  is  larger  than  4.59,  then  the 
system  is  unstable  for  any  a, and  if  a  becomes  larger  than  2,  System  (2.12)  is  always  unstable  for  any 
0. 


From  Eq.  (2.15),  we  define  3k(A)  as  follows: 


9k{ A)  =  /  |1  —  r\dr  -  A, 

Jo 


■{ 


-  A2/2  if  0  <  A  <  1, 

A2/2  -  2A+  1  if  1  <  A. 


(2.19) 


9k( A)  is  also  drawn  in  Fig.  2.1.  It  shows  that  the  system  is  asymptotically  stable  if  A  6  U[ 0.5, 3.32], 
while  this  system  is  asymptotically  stable  if  A  €  £/[0.5, 3.86]. 

•  If  System  (2.12)  is  asymptotically  stable  for  [a,  3}  for  u  =  —x,  then  the  system  is  also  asymptotically 
stable  for  [a/ K,0/K]  when  we  use  u  =  —  Kx  instead  of  u  =  —  x.  This  implies  that  there  exists  K 
for  any  a  and  0  which  stabilizes  the  system. 
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Figure  2.1:  function  g 


limefsec] 


Figure  2.2:  Simulation 


2.3  Multi-Dimensional  Systems 

In  this  subsection,  we  derive  a  sufficient  condition  for  the  multi-dimensional  randomly  sampled  system  Eq 
(2.1)  under  the  input  Eq.  (2.2). 

A  sufficient  condition  for  the  stability  can  be  obtained  from  Prop.  1  using  an  appropriate  matrix  norm 
instead  of  the  absolute  value  operation,  for  example 

||r||  =  [Amax(rr)]1/2  (2.20) 

where  I"  is  the  conjugate  transformed  matrix  and  Amax(T)  denotes  the  maximum  eigenvalue  of  the  matrix 
T.  Note,  however,  that  while  the  stability  of  the  system  (2.1)  or  (2.3)  is  invariant  under  the  equivalent 
transformation  of  the  state  variables,  the  matrix  norm  depends  on  the  transformation 

Ill'll  £  ||T-|IT||. 

This  fact  is  taken  into  account  in  the  following  proposition  which  gives  a  sufficient  condition  for  the 
asymptotical  stability  with  probability  one  of  multi-dimensional  systems. 

Proposition  2  (Sufficient  Condition)  A  randomly  sampled  control  system  (2.1)  is  asymptotically  stable 
if  there  exists  a  non-singular  matrix  T  suck  that 

£[log(||T-1r(A)T||)]<0  (2.21) 

and 

F[log(||T-1r(A)T||)]  <  co.  (2.22) 

The  proof  is  similar  to  the  Sufficiency  part  of  the  proof  of  Prop.  1  except  that  we  must  use 

INII/IMI  <  ||r-lr(Afc)T||  •  ||r-1r(A*_1)r||  ■  •  •  ||r-1r(A0)r||  (2.23) 

instead  of  Eq.  (2.8)  in  the  proof  of  Prop.  1.  Therefore  we  will  omit  it  here.  The  next  example  shows  that 
Conditions  (2.21)  and  (2.22)  are  not  necessary. 

Note  that  for  any  n  x  n  matrix  T  and  any  f  >  0,  there  exits  an  n  x  n  non-singular  matrix  T  such  that  1 

||T-lIT||<  |Amar(r)|  +  c  (2.24) 

while  we  always  have 

r-lIT]|  >  |Amo,(r)|.  •  (2.25) 

Therefore  if  the  sampling  interval  is  constant,  Prop.  2  gives  a  necessary  and  sufficient  condition  for  the 
asymptotical  stability  of  the  system. 

Example  3  Let's  consider  the  stability  of  the  random  varying  autonomous  system  as  follows: 


Zfc+i  =  r*t 

and  assume  T  =  r0  with  probability  p  or  Pi  with  probability  q  =  1  —  p,  where 


To  = 


0  a 
0  0 


1  After  transforming  T  into  a  Jordan  canonical  form  using  Tj ,  use  Tj  =  diag.fl,  S, . . . ,  <5n_1 }  where  if  is  a  sufficient  small 
positive  number  depending  on  «.  T  is  given  as  TiTj 
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and 


0  0 
a  0 


and  a  >  0. 

Note  that  Tg  =  Tj  =  0.  This  means  that  z*  jb  0  for  some  xo  if  and  only  if 


Xk  =  r0r,r0r,  •  •  •  zo 

or 

xk  =  rjEoTiTo  •  •  -z0- 

Therefore 

n  e  f  1  -  2(pg)*/2  i/i  is  even, 

**  =  0  for  any  z0  with  probability  |  j  _  (^{k-i)/7  (f  k  is  odl 

Clearly  the  system  is  asymptotically  stable  for  any  p,  q  and  a. 

Now  we  apply  Prop.  2  to  this  system.  If  we  use  the  following  matrix: 


then  we  have 

£(log(||rt|)]  =  log  a  +  (p  -  q)  log  6. 

This  equation  implies  that  when  a  <  1,  the  system  is  asymptotically  stable  for  any  p  and  q  by  setting  6  =  1 
and  when  a  >  1,  we  can  select  ah  appropriate  6  for  which  the  expectation  becomes  negative  if  p  ^  q.  Hence 
the  system  is  asymptotically  stable  if  p  ^  q  or  p  yb  1/2. 

It  is  easily  to  show  that  the  system  is  asymptotically  stable  in  the  first  moment  if  and  only  ifarpq  <  1  [5] 
and  in  the  second  moment  if  and  only  if  aipq  <  1  [11],  Therefore  neither  the  stability  in  the  first  moment 
nor  the  one  in  the  second  moment  implies  the  condition  of  Prop.  2,  while  it  gives  the  nearest  condition 
among  of  them  for  this  example. 


We  have  the  following  corollary. 

Corollary  2  The  multi-dimensional  randomly  sampled  system  (2.1)  with  n  ^  r  is  asymptotically  stable 
with  probability  one,  if  there  exist  a  positive  number  c  such  that  /(A)  =  0  for  A  >c  and 

£pog(|r(A)|)]  <  0 

where  /(A)  is  the  distribution  function  of  sampling  intervals. 

<  proof  >  , 

For  the  multi-dimensional  system,  Condition  (2.22)  is  not  satisfied  only  if 

T(A)  =  $(A)  +  <F(A)/^  =  0. 

However  this  is  impossible  because  <$(A)  is  a  nonsingular  matrix,  while  'F(A)/<'  is  not  so  for  any  A  if  n  r. 
Therefore  Condition  (2.22)  is  always  satisfied.  <  end  of  proof  > 

Also  defining 

7(A)  =  log(||T-lr(A)T||)  (2.26) 

?(A)  =  /  7(r)dr  (2.27) 

Jo 

we  have  the  following  corollary  from  Coro.  2. 
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Corollary  3  (Bernoulli,  Uniform,  and, Mixed  Uniform  Distributions) 

i.  If  the  sampling  rate  A  is  subject  to  a  Bernoulli  distribution  where  A  =  a  with  probability  p  and  A  =  3 
with  probability  q  =  1  —  p,  then  the  multi-dimensional  randomly  sampled  system  (2.1)  with  n  £  r  is 
asymptotically  stable  with  probability  one,  if 


py(of)  +  n(£)  <  0- 


ii.  If  the  sampling  rate  A  is  subject  to  a  uniform  distribution  U[a,0\,  then  the  system  is  asymptotically 

stable  with  probability  one,  if 

g{0)  -  g{a)  <  0. 

iii.  If  the  sampling  rate  A  is  subject  to  U{a,I3]  with  probability  £  and  to  U (7,  6)  with  probability  1  —  £,  then 

the  system  is  asymptotically  stable  with  probability  one,  if 


(1  _  e)2i2-lM  <  0. 

0-a  <5-7 


This  corollary  is  trivial  from  Prop.  2  and  Coro.  2. 


2.4  Design  of  Feedback  Gains 

In  the  above,  we  discussed  the  stability  of  randomly  sampled  control  systems  assuming  that  the  feedback 
gain  K  was  given.  In  this  section,  we  discuss  the  design  procedure  to  select  the  feedback  gain  I<  and  matrix 
T  to  make  the  system  stable. 

If  a  continuous  time  system 

=  +  (2.28). 

is  controllable,  it  is  well  known  that  the  discretized  system 

x*+i  =  $(A)x*  +  tf(A)u*  (2.29) 

is  also  controllable  for  almost  any  sampling  interval  A  [3].  We  can  then  assign  poles  {Aj,  1  =  1, 2, . . . ,  n)  to 
the  system  (2.29)  using  an  appropriate  state  feedback  with  probability  one  if  the  poles  {A,}  are  symmetric 
with  respect  to  the  real  axis.  So  far,  a  lot  of  pole  assignment  algorithms  are  proposed.  Among  them, 
Hikita’s  pole  assignment  algorithm(8]  is  very  convenient  for  us  because  it  gives  us  not  only  the  feedback 
gain  K  but  also  the  transformation  matrix  T  which  we  can  use  to  check  the  stability  by  using  Prop.  2. 
Hence  we  modify  it  as  follows: 

t 

(Algorithm] 

step  (i)  For  a  given  {A;},  find  the  r-dimensional  vectors  {£,}  where  i  =  1,2,...,  n  which  make 
the  matrix  T( A)  =  (vi  :  :  t>n]  non-singular  with  Vi’s  being  defined  by  (he  following 

equations,  in  which  $  =  $(A)  and  =  $(A). 

•  if  A,-  is  a  real  number,  then 


Vi  =  (<&-A,/n)-1tf&.  (2.30) 

•  if  A;  and  Aj+j  are  conjugate  complex  numbers  or,-  ±  j/3i,  then 

vi  =  ~  k2i&+i 

wi  +  l  =  Vuti  +  V2i£;+1  (2.31) 
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where 


VU  =  {($  -  CLiIn )2  +  fifing*  ~  <*,/»)* 

V2i=  {(<5  —  Ot,/n)2  +Piln}~l0i'&- 

step  (ii)  Feedback  gain  K  is  given  as  follows. 

/C(A)  =  -Ki:---:^]T(A)-1.  (2.32) 


It  is  easy  to  show  that  if  A*  is  a  real  number 


(«  +  *K)vi  =  A  iVi 


and  if  A,-,  A,+1  =  a{  ±  j0{, 


($  +  $ K)v{  =  otiVf  -  0iVi+ 1 
($  +  ^/C)uj+i  =  0{v  i  +  Qf,yi+1. 

This  implies 

||T-1(A)r(A)T(A)||  =  <r(r(A)). 

Hence  we  can  use  matrices  T(A)  and  A'(A)  to  check  the  stability. 


(2.33) 

(2.34) 

(2.35) 

(2.36) 


2.5  Two  Dimensional  Systems 


We  view  a  robot  manipulator  as  a  component  of  a  large  system,  such  as  a  space  station.  The  robot  controller 
communicates  with  the  other  components  of  the  system  to  achieve  cooperative  actions.  Communication 
between  components  is  considered  to  have  a  longer  delay  than  that  within  a  component.  We  assume  that 
robot  controller  has  an  inner  feedback  loop  which  compensates  the  nonlinearity  of  manipulator  dynamics 
and  operates  independently  of  the  other  part  of  the  system.  The  robot  dynamic  system  together  with  the 
inner  feedback  loop  becomes  a  linear  system,  It  is  feasible  to  treat  the  robot  manipulator  subsystem  as  a 
linear  system  when  integrating  and  communicating  with  the  other  components.  For  example,  if  we  use  the 
nonlinear  feedback  controller  developed  in  (2j,  we  have  r  (=DOF  of  manipulator)  decoupled  two-dimensional 
linear  systems 


*(0  = 


o  i 
0  0 


*(0  + 


0 

1 


,«(0 


(2.37) 


where  x(t)  =  (e,-(i),  e,(i))  is  the  error  vector  for  the  i-th  component  of  outputs  aijd  u(t)  is  the  corresponding 
input  for  this  component  of  outputs.  If  the  task  is  specified  in  joint  space  (the  joint  space  control),  the  i-th 
component  of  output  is  simply  the  displacement  of  the  i-th  joint  and  the  error  vector  is  composed  of  the 
joint  position  error  and  joint  velocity  error.  This  system  also  can  be  used  as  an  approximation  subsystem  of 
a  robot  manipulator  which  Is  controlled  by  a  randomly  sampled  computed  torque  controller  or  a  randomly 
sampled  simple  computed  torque  controller  as  shown  in  the  next  chapter,  where  the  i-th  component  of  out 
put  is  the  joint  position  error  and  joint  error,  precisely. 

We  now  study  the  asymptotical  stability  of  this  system  under  the  random  sampling  rate.  The  corre¬ 
sponding  discrete  time  system  is  easily  obtained  for  a  sampling  interval  A  as  follows. 


zjt  +  i 


1  A* 

0  1 


+ 


*E/2 

A  t 


(2.38) 


We  apply  the  algorithm  given  above  to  this  system  directly.  Then  we  have  the  following  proposition. 
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Figure  2.3:  function  7 (8,  1)  and  g(8 ,  1) 


Proposition  3  (PD  Controller)  Assume  that  {A,}  =  {A|,A2}  where  Ai  /  A2,  then  we  have 
K(A)  =  ((Ai  +  A2  -  A]  A2  -  1)/A2,  (Ax  +  A2  +  Ax  A2  -  2)/(2A)), 


T(A)  = 


~ 4iA(l  +  Ai)  — 6A(1  +  A2) 
2^(1 -Ax)  26(1  -Ax) 


and 

where  B  =  A/A. 


7(A,  A)  =  j(8, 1), 


The  proof  is  obtained  by  direct  calculation.  This  proposition  implies  that  the  function  7(A,  A)  is  the 
same  as  the  function  7 (8, 1)  if  we  use  K(A)  =  (kp/A2,kv/A)  instead  of  A'(l)  =  (kp,kv).  Therefore  we 
have  g( A,  A)  =  A g(9, 1)  for  the  same  K( A).  This  fact  is  very  useful  to  design  the  feedback  gain.  This  will 
be  shown  by  examples. 

Fig.  2.3  shows  7 (8, 1)  and  g(9,  1)  for  Ax  =  0.4  and  A2  =  0.7,  where  we  have 


/<■(!)  =  -(0.18,0.81),  and  T(l)  = 


-0.759  -0.943  / 
0.651  0.333  ’ 


(2.39) 


and  was  used  to  make  the  norm  of  column  vectors  of  T  matrix  be  equal  to  one. 


2.5.1  Bernoulli  Distribution 

Let’s  assume  that  the  sampling  interval  is  subject  to  Bernoulli  distribution,  i.e.  A  =  a  with  probability  p 
and  A  =  p  with  probalility  q,  where  a  <•/?,  0  <  p  <  1,  and  q  =  1  —  p.  The  sufficient  stability  condition  is 
given  as  follows. 

P~f(a/ A,  1)  +  qy(P/ A,  1)  <  0.  (2.40) 

Note  that  if  A  >  /?/ 1.96(=  A")  then  the  system  is  asymptotically  stable  for  any  a  because  y(9 , 1)  <  0  for 
any  8  <  1.96.  But  we  are  generally  interested  in  the  smallest  A  because  it  gives  us  the  fastest  response. 
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Fig.  2.3  shows  that  the  function  7(0,1)  reaches  the  minimum  value  -0.417  at  0  —  1.35.  Let  0*  be  the 
point  which  satisfies  the  following  equation. 

7(0’,  1)  =  2  x  0.417. 

7 

Then  it  is  clear  that  A  must  be  greater  than  A min(=  for  Eq.  (2.40). 

A  suitable  value  of  A  can  be  found  from  the  range  Amtn  <  A  <  A*  by  a  trial-and-error  method  using 
Fig.  2.3  or  Table  2.1  which  gives  pairs  of  {0i,02}  such  that  y(9\,  1)  =  7(02, 1)' 

(i)  Calculate  a  =  —(q/p)~f{0/  K,  1). 

(ii)  Find  {0i,02}  such  that  7(0i,  1)  =  7(02, 1)  <  a  using  Fig.  2.3  or  Table  2.1. 

(iii)  Check  0j  <  a/ A  <  0j.  If  so,  calculate  K( A).  If  not  so,  go  back  to  step  (i)  with  another  A. 

For  example,  if  a  =  10  msec,  0  =  30  msec,  and  p  —  0.75,  then  0*  is  about  3.64  and  Am,„  =  8.24  msec, 
while  A’  =  15.3  msec.  If  we  select  A  =  11  msec  then  ^y(0/A,  1)  =  -0.278  and  a/A  =  0.91.  Therefore 
we  can  try  the  6-th  row  of  Table.  2.1,  and  we  have  0i  =  0.84  <  0.91  <  02  =  1.68.  Hence  the  system  is 
asymptotically  stable  for  K  =  —(1488,73.64). 

2.5.2  Uniform  Distribution 

Now  assume  that  A  is  subject  to  a  uniform  distribution  U[a,0\.  The  sufficient  condition  of  the  asymptotical 
stability  with  probability  one  is  given  as  follows: 

<?(a/A,  1)  >  g{0/L,\). 

The  function  g{6, 1)  has  its  minimum  value  at  0  =  1.96.  Now  we  define  A’  =  /?/ 1 9.6  and  Am<n  =  3/ 2.89. 
If  A  >  A’,  then  the  above  sufficient  condition  is  satisfied  for  any  a.  Therefore  the  system  is  asymptotically 
stable  if  A  >  A*.  On  the  other  hand,  if  A  <  Am,„,  then  the  above  condition  is  not  satisfied  for  any  a. 

Table  2.1  also  gives  pairs  of  {03, 0^}  and  the  ratio  03/04  such  that  ?(03,  1)  =  <7(04, 1).  If  there  is  a  pair 
{03,04}  such  that  a/3  >  03/04,  then  the  system  is  asymptotically  stable  for  the  /v(A)  where  A  =  a/03. 
Therefor  we  can  determine  A  easily  using  this  table  as  follows: 

(i)  Calculate  a  =  a/0. 

(ii)  Find  a  pair  {03,04}  in  the  Table  2.1  such  that  a  >  03/04-  . 

(ii)  Calculate  A  =  a/03  and  K( A).  ( 

Now  assume  that  a  =  10  msec  and  0  =  30  msec,  then  we  have  A"  =  15.3  msec,  Amj„  =  10.38  msec, 
and  a/0  =  1/3  >  0.273  in  the  Table  2.1.  Therefore  sve  can  use  a/A  =  0.75  and  A  =  13-33  msec. 
Hence  the  system  is  asymptotically  stable  with  K  =  -(1065,62.31)  if  0  <  36.7  msec.  Table  2.2  shows 
the  IAE  (Integration  of  Absolute  value  of  the  Error)  for  fifty  random  streams  with  the  initial  condition 
z(0)  =  ( 1 .0, 0)T .  The  table  shows  that  when  0  >  40msec,  the  STD  (STanderd  Deviation)  and  the  maximum 
values  of  IAE  for  the  velocity  error  e,(f)  become  very  large  compared  to  the  cases  where  0  <  35  msec.  This 
means  that  the  system  is  still  stable  but  there  is  a  large  vibration  in  the  response  for  A  >  40  msec.  It  is 
interesting  since  A  selected  above  assures  the  asymptotically  stability  for  0  <  36.7  msec. 
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Figure  2.4:  Simulations  for  Bernoulli  Distribution,  Uniform  Distribution  and  Mixed  Uniform  Distribution 


2.5,3  Mixed  Uniform  Distribution 

Next  we  assume  that  A  is  subject  to  a  uniform  distribution  U[a,0]  with  probability  t  and  to  U\ji,v]  with 
probability  1  —  t.  The  sufficient  condition  is  given  as  follows: 

r.  _  M0/&,  0  1.)  ,L  vg(^/A, 1)  -  g(p/A, 1) 

0/A  —  a/A  v/A  —  p/A 

Though  the  selection  of  A  becomes  a  little  difficult,  we  can  use  the  following  procedure  to  estimate  an 
appropriate  A: 

(i)  Define  a  =  (a  +  0)/2.O,  0  =  (p  +  ^)/2.0,  p  =  e,  and  q  =  1  —  p. 

(ii)  Determine  A  using  the  procedure  in  Exam.  1  for  a  =  a  and  0  —  0. 

(iii)  Check  the  condition.  If  satisfied,  calculate  K{  A).  If  not,  try  another  value  for  A. 

Now  assume  that  A  is  subject  to  W[5msec,  15  msec]  with  probability  z  =  0.75  and  to  W(20  msec,  40msec] 
with  probability  0.25.  Then  we  have  a  —  10  msec,  0  =  30  msec,  p  =  0.75,  and  q  =  0.25.  If  we  use 
A  =  11  msec  from  the  result  of  Exam.  I,  then  we  have  E  =  —0.04  <•  0.  Therefore  the  system  is 
asymptotically  stable  for  the  same  K  =  —(1488,73.64).  ' 

Fig.  2.4  shows  the  simulations  of  ,r(0  for  three  cases  discussed  above  where  r(0)  =  ( 1 .0 , 0 )T - 

2.6  PID  Controller  and  PD  Controller  with  One  Step  Delay 

Here  a  PID  controller  and  a  PD  controller  with  one  step  delay  for  a  random  sampled  system  are  discussed. 

2.6.1  PID  Controller 

We  consider  a  linear  time-constant  control  system 

i(t)  =  -4x(t)  +  Bu(t)  +  d, 

i/(0  =  Cx{t), 
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(2.41) 

(2.42) 


Cl 

II 

^(03, 1)  = 

ff(*4,l) 

y(0,  i) 

9 1 

'92 

ff(M) 

9 A 

9z/9a 

0.00 

0.00 

1.96 

0.000 

0.00 

2.88 

o.ooo 

-0.05 

0.18 

1.92 

-0.009 

0.25 

2.87 

0.087 

■dm 

mail 

1.89 

0.50 

2.84 

0.176 

msssm 

0.46 

1.83 

0.75 

2.78 

0.270 

■BWil 

0.58 

1.79 

|gillr*« 

1.00 

2.69 

0.372 

imtmM 

0.71 

1.73 

H!£U!I 

1.25 

2.56 

0.488 

0.84 

1.68 

gilMl 

1.50 

2.39 

0.628 

0.98  1 

1.60 

gilEWil 

1.75 

MU 

0.806 

mm 

1.18 

1.48 

■DJEfcM 

1.80 

2.12 

0.849 

Table  2.1:  6\,  0o  ,03,  and  9 « 


MEAN 

STD 

MAX 

0 

edt) 

e,-(<) 

MO 

edt) 

edt) 

edt) 

25 

0.0531 

0.9999 

0.0022 

0.0011 

0.0572 

1.0020 

30 

0.0511 

1.0039 

0.0043 

0.0200 

0.0561 

1.1310 

35 

0.0498 

1.0198 

0.0060 

0.0515 

0.0589 

1.2370 

40 

0.0509 

1.2418 

0.0077 

0.4305 

0.0717 

2.7051 

45 

0.0709 

2.6492 

0.0638 

4.4592 

0.4354 

30.276 

Table  2.2:  IAE  for  f/[10  msec,  (3  msec] 


where  d  and  y(<)  are  an  n-dimensional  constant  disturbance  and  an  r-dimensional  output  vector,  respec¬ 
tively.  We  assume  that  this  system  is  controllable  and  observable.  Then  a  continuous- time  PID  controller 
is  given  as  follows: 


i(<)  =  y-y(t) 

u(t)  =  K\x(t)  +  K7z(t), 


where  y  is  an  r-dimensional  constant  reference  vector.  It  is  well  known  that  if  and' only  if 


det 


.4  B 
C  0 


#0, 


there  exist  feedback  gains  K\  and  Kj  such  that  x(t)  — *  y  as  i  — •  cc  for  any  x(0),  y  and  d  [3]. 
Now  we  consider  a  randomly  sampled  system  controlled  by  a  PID  controller  as  follows: 


(2.43) 

(2.44) 

(2.45) 


•=*+i  =  $(At)rj;  4-  (Afc)ui;  +  d  (2.46) 

y*  =  C*k  (2.47) 

Zfc+i  =  -i  +(y-jr*),  (2.48) 

a*  =  Kixk  +  K-2Zk,  (2.49) 

)  =  (  I*  )  +  +  (  j  )  .  (2.50) 

(2.51) 
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Then  we  have 


2fc  +  l 


where 


$(A*)  = 


«(A  t)  0 
-C  U 


*(A*)  = 


¥(A*) 

0 


I<  =  [Ki  A'o). 


Now  we  assume  that  Condition  (2.45)  is  satisfied,  then  it  is  easy  to  show  that  for  almost  all  A*, 


det 


®(A  k)-In  ¥(A*) 
-C  0 


#0 


(2.52) 


is  satisfied,  too.  This  implies  that  System  (2.50)  is  controllable  for  almost  all  At-  Hence  we  can  apply  the 
algorithm  described  in  Section  2.4  to  choose  feedback  gain  K  and  transformation  matrix  T  for  which  the 
randomly  sampled  system  becomes  asymptotically  stable  with  probability  one.  Then  we  have 

Iki+i  —  Zill  —*0,  or  a:*  — *  y  as  k  — *  oo  with  probability  one.  (2.53) 


For  t%vo-dimensional  system  (2.37),  we  have 


*(At)  = 


■  1 

At 

0  ■ 

frt-j 

to 

0 

1 

0 

,  tf(At)  = 

At 

-1 

0 

1 

0 

(2.54) 


where  we  assumed  C  =  [1  0].  By  applying  the  algorithm  in  Section  2.4,  we  have  following  proposition: 


Proposition  4  (PID  Controller)  Lei  T(1.0)  and  K- (1-0)  be  given  by  the  algorithm  in  Section  2-4  for 
extended  system  (2.50)  by  using  A  =  1.  If  we  use  the  algorithm  for  a  A,  then  we  have 


f(A)  = 


Ati 
tj  , 
.  At3  . 


.K( A)  =  [fci/A2  k,/ A  fc3/A2], 


and 

7(A,A)  =  7(A/A,1.0) 


where 

7(A,  A)  =  log(||f-‘(A){$(A)  +  tf(A)/?(A)}T(A)||), 
t,-  and  ki  are  the  i-th  row  vector  ofT(  1.0)  and  the  i-th  element  of  K{  1.0),  respectively. 


The  proof  is  given  by  direct  calculation.  Therefore  we  can  use  the  similar  procedures  to  ones  described  in 
above  section  to  determine  the  appropriate  A  for  given  sampling  interval  distribution. 

Example  4  Fig.-  2.5  shows  the  7(A,  1.0)  and  g( A,  1.0)  for  (A,}  =  {0.95, 0.7, -0.4}  and 


T(1.0) 


-0.0499  -0.0715  -0.0294 

0.0026  0.0252  0.0252 

-0.9986  -0.2382  -0.0491 


The  feedback  gam  is  K{  1.0)  =  [-0.221,-0.840,0.009).  Note  that  y(A,  1.0)  becomes  positive  for  small 
A.  This  is  due  to  the  change  of  the  direction  of  eigen  vectors.  For  A  6  £7(10  msec,  30  msec],  we  have 
A  =  14.29  msec  and 


Zk+ 1  =  zt  +  (y-y*), 

uk  =  — (1083,58.80]zt  +  44.10zt. 

Fig.  2.6  shows  a  simulation  for  x0  =  0  and  y  =  1.0.  Disturbance  d  —  (0.0, 1.0)T  is  applied  to  continuous 
time  system  (2.42).  The  result  shows  a  smooth  convergence  into  y. 
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2.6.2  PD  Controller  with  One  Step  Delay 

If  Ufe  is  calculated  using  not  zjt  but  st-i',  then  we  have  a  one  step  delay  controller.  This  is  common 
situation  for  Robot  controllers.  A  simple  way  to  compensate  the  delay  for  constant  sampling  time  system 
is  to  use  a  controller  as  follows; 

tit  =  tf(*(A)art_i  +  tf(A)ut_i).  (2.55) 

Here  we  consider  the  stability  of  the  system  controlled  by  the  above  controller  under  random  sampling 
intervals.  Namely,  we  use  a  PD  controller  with  one  step  delay  as  follows; 


ut  =  A(A)($(A)zt-i  +  tf(A)ut-i), 


(2.56) 


where  K’(A)  is  determined  using  the  algorithm  in  Section  2.4  for  a  A.  Of  course,  T(A)  is  also  determined 
at  the  same  time.  Then  we  have  following  extended  equation: 


(  **+i  \  _  $(At)  *(At)  /  xk  \ 

V  tifc+i  )  [  K( A)$(A)  K{ A)^(A)  J  V  / 


(2.57) 


If  we  use 


T(A)  = 


'  T(A) 
A(A)T(A) 


0 

Sir 


where  5  is  a  small  real  number,  then  we  have 


r-*(A) 


■  $  y  ■ 
K<b  K* 


f(A)  = 


f~lrf  st 
-k{t-r)f/s  k('i>  -  'S) 


(2.58) 


(2.59) 


where  =  <5(A),'t  =  'i'(A), K  =  K(A),T  =  T( A)  and  r  =  $  -F  '£/(.  <$,'£,  and  f  are  corresponding 
matrices  for  A  =  A,  respectively.  Therefore  we  can  check  the  stability  of  the  system  using  matrix  T( A). 
For  the  two-dimensional  systems  such  as  Eq.  (2.37),  we  have  proposition  as  follows: 


Proposition  5  (PD  Controller  with  One  Step  Delay)  For  two-dimensional  system  (2.37),  we  have 

-?(A,A)  =  7(A/A,1.0), 


where 


7(A,A)  =  log(|| 


f-lrt  sf-Lv 

L  k{t  -  r)f/s  k{*  - '?) 


The  proof  is  also  straightforward.  > 

Example  5  Fig.  2.7  shows  7(A,  1.0)  and  g( A,  1.0)  for  {A,-}  =  (0.95,0.90)  and  6  =  0.005.  Then  we  have 
K(1.0)  =  -[0.0050, 0.1478]  and 


T(  1.0) 


-0.9986  -0.9945  0.000 
0.0513  0.1047  0.000 

-0.0026  -0.0010  0.005 


We  must  use  eigenvalues  near  to  1  +  ;0  in  order  to  get  a  wide  minus  zone  of  function  7(A,  1.0)  and  this 
results  m  rather  small  feedback  gain  A(1.0).  Now  assume  that  usually  the  controller  works  every  5  msec 


23 


Y 

g 


0  12  3 

deitafsec] 


Figure  2.5:  t(A,  1.0)  and  p(A,  1.0)  for  PID  Controller 


with  probability  0.9  but  it  must  sometimes  wait  for  from  10msec  to  20  msec  with  probability  0.1.  Then  the 
system  is  asymptotically  stable  if 


n  .  a(20  msec,  A)  -  <?(10  msec,  A) 

0.9  *  7(5  msec,  A)  +  0.1  *  - - — — - — -  <  0, 

10  msec 


or 

1  0.9  *  7(0  msec/ A,  1.0)  +  ^ {g(20  msec/ A,  1.0)  -  ^(10  msec/ A,  1.0)}. 

If  we  use  A  =  2.5  msec,  it  is  shown  that  the  above  inequality  is  satisfied  and  we  have 


Ui  =  -(800.0,61.21)^-1  -  0.1503u*_i. 

Fig.  2.8  shows  a  simulatton  for  xq  =  (1,0)T.  This  figure  also  shows  the  smooth  and  rapid  convergence  to 
zero  state. 


time[sec] 


xl 


o  0.1  0.2  0.3  ■  0.4  0.5 

time[sec] 


Figure  2.8:  Simulation  for  PD  Controller  with  One  Step  Delay 
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Chapter  3 

Robot  Manipulators 


In  this  chapter,  we  consider  the  asymptotical  stability  with  probability  one  of  randomly  sampled  robot 
systems.  Dynamic  equations  of  robot  systems  are  usually  described  by  non-linear  functions  and  controllers 
also  have  non-linearity.  Therefore,  first  of  all,  these  equations  are  linearized  along  desired  trajectories  to 
time-variant  multi-dimensional  linear  systems  and,  next,  the  results  given  above  are  applied  to  them. 

Here  we  consider  three  kinds  of  controllers: 

•  PD  controllers  with  feedforward  terms  (16] , 

•  Conputed  Torque  Controllers  [20]  [17], 

•  Simple  Computed  Torque  Controller, 

and  discuss  their  effectiveness  under  the  random  sampling  situation.  , 

,  » 

3.1  Stability  Condition 

In  this  section  we  will  consider  the  stability  of  randomly  sampled  robot  control  systems.  The  dynamic 
equation  of  a  robot  and  the  equation  of  a  controller  are  given  as  follows. 

*(0  =  /(*(*),«*)  =  /i(-»(0)  +  /a(*(*))«fc  (3.1) 

uk  =  h(x(t t ) ) ,  tk  <  t  <  =  (tk  +  At)  (3.2) 

where  x(t)  and  uk  are  the  state  vector  and  the  input  vector  of  the  robot,  respectively.  /  and  h  are  smooth 
non-linear  functions.  We  assume  that  a  planned  trajectory  (z(t)  :  to  <  <}  is  given  and  also  assume  that 


Hih)  =  /(z(**).fit) 

tit  =  Mi(<Jt))- 


Then  using  the  following  variables 


(3-3) 

(3.4) 


z(f)  =  x(t)  +  6x(t) 
Ufc  =  tit  +  <5ut 


27 


(3-5) 

(3.6) 


and  linearizing  the  above  equations  around  the  trajectory,  we  have 


6i(t)  =  A(t)Sx(t)  +  B(t)6ut  -f  d(t)  (3-7) 

Suk  =  Ct6x(ik)  (3.8) 

where 

A  (0  =  [df/dx](x(t),uk) 

B(t)  =  Mm 
ck  =  [dh/dx]{x{tk)) 

d(t)  =  f(x(t),uk)  -  i(0 
df/dx  =  [df/dxi  :  :  df/dxn\. 


d(t).  is  a  disturbance  due  to  using  sampling  controllers  for  the  system  and  it  is  omitted  from  the  system 
equation  in  the  following  discussion  of  the  stability. 

<5x(4+i)  can  be  calculated  for  a  given  6x(ik)  and  as  follows: 


<5z(4+i)  =  $(4+i,4)£*(4)  +  'P(4+i,4)£ufc-  (3.9) 

where  <I>(4+i ,  tk)  is  the  unique  solution  of 

£‘P(t,tk)  =  A(l)$(t,lk)  $(4,4)  =  /„ 

and  $(4+1,4)  is  given  by 

$(4+i,4)  =  /  $(4+i,0  B(t)dt. 

Jt k 

By  substituting  6uk  of  Eq.  (3-8)  into  the  above  equation,  we  have 

<5z(4+i)  =  r(ft,  At)<5x(4)  (3,10) 

where 

r(*fci  At)  =  $(4+i, 4)  +  $(4+i,  tk)Ck. 

We  now  define  a  random  variable  7  for  any  non-singular  matrix  T  as  follows. 

7(4,  Afc)  =  iog(||T-1r(fJt,  Afc)T||).  (3.11) 

Note  that  7(4,  Ak)  is  a  function  of  A0,  •  •  • ,  At,  therefore  7 (4,  At)  and  7 (4+1,  At+i)  are  not  independent 
of  each  other,  even  though  we  have  assumed  that  At’s  are  in  this  paper.  On  the  other  hand,  we  have 

7(4,0)  =  0  (3.12) 

for  any  tk  >  t0.  This  denotes  that  the  effects  of  A,-,  i  =  0,  ■  •  • ,  k  —  1  are  not  accumulated  on  7(4,  At),  and 
we  can  expect  7(4,  At)  and  7(4+1,  A/)  to  become  independent  statistically  as  l  increases. 

We  now  introduce  some  definitions. 
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Definition  2  (weak  correlation)  The  robot  system  is  weakly  correlated  along  the  trajectory  (r(<)  :  to  < 
t),  if  there  exists  an  integer  L  such  that  .. 

Cor[y(tkl  Ai+I)]  =  0 

for  k  =  0, 1,2,  •••  and  integer  l  >  L,  where  Cor[a,b]  means  the  correlation  coefficient  between  random 
variables  a  and  b  and  tk+i  =  tt  +  A*  + - b  At+i_i, 

Definition  3  The  randomly  sampled  robot  system  is  asymptotically  stable  with  probability  one  along  the 
trajectory  {x{t)  :  to  <  t },  if 

Prob[  lim  ||<5x(ij.)||  =  0]  =  1 

k—*co 

for  any  <5x(to). 

We  then  have  the  following  proposition. 

Proposition  6  (Stability  of  Robot  Manipulator)  Assume  that  the  robot  system  is  weakly  correlated 
along  the  trajectory  (x(t)  :  <o  <  <}.  The  robot  system  is  asymptotically  stable  with  probability  one  along  the 
trajectory  ,  if  there  exist  two  numbers  a  and  ji  <  0  such  that 


£(7(f,A)|f]  <  n  <  0 

£({7(*.  A)}2jt]  <  <r2  +  /j2  <  co 

for  any  t  >  t0,  where  £[a|6]  is  the  conditional  expectation  of  a  given  b. 

<  proof  > 

From  Eq.  (3.10),  we  have 


Therefore 


Note  that 


bz(tk)  —  r(<i_l,At_,)---r(<o,A0)^(<o). 

/oy(||T-l5x(4)||)<log(l|T-1<5z(t0)||)  +  ^7(<,,Ai). 

1=0 


£[7(fj,Aj)]  =  E[B[y[ti,  Ai)|<,)]  <  pt 

K[-y(<,,  At  )]  =  E[E[{l{U,  A,)}2|f,]]  -  (^(^A,))}2  <  <r2 

for  i  <  j.  Now  define  zk  as  follows. 

Zk  ~ 

i=0 

then  we  have 


E[zk]  <  A < 

rw  < 


(3.13) 
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where  7i  =  y(t{,  A,)  and  we  used  |Cov{7;,-7;]|  <  \jCov\fli]Cov{‘(j\  <  <7 2  where  Cou[7,-,7;)  is  the  covariance 
of  7,-  and  7 j.  This  means  that  V[zk\  -*0  a s  h  —  co,  and  by  Tchebycheff’s  inequality  we  have 

Prob[zk  <  p/2)  >  Prob[\zk  -  E[Zk] |  <  p/ 2]  >  1  - 


Hence  we  have 

logdlT-1^)!!)  -  -CO 

with  probability  one  for  any  xo  as  k  — ►  co.  □ 

Note  that  the  condition  of  the  above  proposition  does  not  depend  on  <0  at  all.  Therefore  if  the  propo¬ 
sition  holds,  we  have 

lim  ||<5ar(<;)||  =  0 
<—*00 

with  probability  one  for  any  fiz(i'0),  (t'0  >  <o),  where  t'k  —  t'Q  +  Af,  + - f  Aj.  _  L . 

We  now  define 


7  (t,T)dr 


then  we  have  the  following  proposition. 


(3.14) 


Proposition  7  (Bernoulli,  Uniform,  and  Mixed  Uniform  Distributions)  Assume  that  the  robot  sys¬ 
tem  is  weakly  correlated  along  the  trajectory  {z(t)  :  to  <  t},  and 

£({7(f,A)}2|<]  <  <t2  <  co 


for  any  t  >  to- 

i.  If  the  sampling  rate  A  is  subject  to  a  Bernoulli  distribution  where  A  =  a  with  probability  p  and  A  —  3 
with  probability  q  =  1  —  p,  then  the  randomly  sampled  robot  system  is  asymptotically  stable  along  the 
trajectory  with  probability  one,  if  there  exists  a  negative  number  p  such  that 


py{t,a)  +  qy{t,P)  <  p  <  0 


for  any  t  >  t0. 

ii.  If  the  sampling  rate  A  is  subject  to  a  uniform  distribution  U[a,/3],  then  the  system  is  asymptotically 
stable  with  probability  one,  if  there  exists  a  negative  number  p  such  that 

g(t,-p)  -  g(t,a)  <  p  <0 


for  any  t  >  to- 

iii.  If  the  sampling  rate  A  is  subject  toU[a,p]  with  probability  t  and  toU[y,6 ]  with  probability  1  —  c,  then 
the  system  is  asymptotically  stable  with  probability  one,  if  there  exists  a  negative  number  p  such  that 


for  any  i  >  io- 


_9(i,0)  ~  g(*.a)  +  ^  -  g{t, 7) 

(3  —  a  '  <5  —  7 


<  p  <  0 


In  practical  situation,  the  dimensions  of  the  state  vector  and  the  input  vector  of  robot  systems  are 
different  (n  =  2 r,  in  general),  the  trajectory  is  defined  in  a  finite  time  inter val  [<o,t/]t/  -  to  <  co  and  the 
sampling  intervals  have  a  finite  upper  limit.  We  have  the  following  corollary  for  such  case. 
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Corollary  4  If  the  dimensions  of  the  state  vector  and  the  input  vector  of  the  robot  system  are  different, 
the  trajectory  is  defined  in  a  finite  time  interval  [to,  tj\  (</  —  <o  <  oo)  and  there  is  a  positive  number  c  such 
that  /(A)  =  0  for  ant  A  >  c,  then  there  exists  a  finite  number  a  such  that 

E[{y(t,  A)}2|<]  <  cr2  <  co 


where  for  any  t  €  o ,  i / ] - 


<  proof  > 

We  denote  set  x  [0,  cj  as  T  in  this  proof.  Clearly  T  is  a  closed  set  in  a  usual  phase.  Therefore 

there  is  an  upper  bound  a  such  that  ||T-Ir(f, A)T||  <  a  for  all  (t,A)  €  T  because  of  continuity  of  the 
function. 

Just  same  as  multi-dimensional  time-invariant  system,  y(t,  A)  becomes  infinite  only  if 


r(t,A)  =  ${t  +  A,t)  +  <!>(t  +  A,t)K  =  0. 

$(t  -f  A ,t)  is  nonsingular  for  any  (t,  A)  while  ^(f  +  A ,t)K  is  not  so  for  any  (f,  A)  if  n  ^  r.  Therefore 
j|T-Ir(t,  A)T||  ^  0  for  any  (f,A)6T  =  [<0i  tj]  x  [0,  cj.  If  there  is  a  sequence  of  pair  (f,-,  A,),  i  =  1,2,3--- 
such  that  ||T“1r(<,-,  A,-)T||  —  0  as  i  -*  oo,  there  is  a  accumulating  point  (<a,Aa)  which  belongs  to  T 
because  the  set  is  closed.  This  implies  r(fa,  Aa)  =  0  and  contradicts  the  fact  shown  above.  lienee  there  is 
a  lower  bound  b  >  0  such  that  ||T-1  r(f,  A)T||  >  6.  Therefore  we  have 

£({7(<.A)}2|f]  <  max  {{log  |a|}2,  {log|f>|}2}  <  oo. 


□ 


3.2  Linearized  Systems,  Controllers  and  Matrix  T 

The  typical  dynamic  equation  of  a  robot  manipulator  is  given  as  follows: 


r  =  M(q)q+C(q,q)  + B(q)  +  G(q) 
-  E(q,q,q) 


(3.15) 


where  r  is  the  joint  force/torque  vector,  q  the  joint  variable  vector,  and  M,  C,  B,  and  G  correspond 
to  the  inertia  tensor,  the  centrifugal  and  Coriolis  force,  the  viscous  friction,  and  the  gravitational  force, 
respectively.  Coulomb  forces  are  omitted  because  we  consider  the  linearization  of  the  equation  here. 

Then  the  robot  dynamic  equation  (3.15)  is  linearized  along  a  trajectory  {q[t),q(t),  ?(0}  at  33  follows. 

’  t 

Si  =  A(t)6x(t)  +  B(t)6uk  '  (3.16) 

where 


Sx 

A(l) 


q  -  q 

q-q 

0 

—  M~l(dR/dq) 
0 

M-1 


Ir 

- Xf~l(dR/dq ) 


h{q{tk),q(tk),q{tk))  -  h(q(tk),  q(tk),q(tk)) 


B(t) 

Suk 
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M  =  M(q(t)),  and  partial  deviatives  are  calculated  at  (q(t),q(t),q(t)). 

6uk  is  obtained  for  a  PD  controller  with,  a  feedforward  term,  a  computed  torque  controller,  and  a  simple 
computed  torque  controller,  respectively,  as  follows: 

( t )  A  PD  controller  with  a  feedforward  term 
The  control  law  is  expressed  as  follows. 

Ujt  =  Kp(q(tk)  -  q(tk))  +  -Ku(<2(ijt)  -  q(tk))  +  Sfe  (3.17) 

where  KP  and  iv„  are  the  proportional  and  differential  gains  respectively.  Then  we  have  the  linearized 
equation  for  the  input  as  follows. 


Suk  =  CkSx(tk)  (3.18) 

where 

Ck  =  ~[KP  Kv). 

(ii)  /I  computed  torque  controller 

The  control  law  is  expressed  as  follows. 

Uk  =  R(q(ik)>q(tk),q’{tk))  (3.19) 

where 

9*(*ifc)  =  ?(<*)  +  Kp(i(tk)  ~  q{tk))  +  Kv(q(tk)  -  q{tk)) 

and  Kp  and  Ku  are  the  proportional  and  differential  gains,  respectively. 

For  this  input,  we  have 

Suk  =  CkSx(tk)  (3.20) 

where 

Ck  =  [dR/dq  -  MkKP  dR/dq  -  MkKv } 

Mk  =  :V/(<j(ffc)),  and  partial  deviatives  are  calculated  at  {q(tk),q(tk),q{tk))- 

( iii )  A  simple  computed  torque  controller 
If  we  use 

«*  =  R(q(tk),q(tk),qm{tk)) 
or 

=  Mk{Kp(q(tk)  ~  q(tk))  +  Kv(${tk)  -  ?(**))}  +  ujt 

then  we  have 


where 


6uk  =  CkSx(tk) 


Ck  =  -Mk[Kp  K,j}. 


(3.21) 


Now,  a  method  to  determine  matrices  Kp,  Kv  and  T  is  to  use  the  algorithm  shown  in  the  above  section 
for  a  multi-dimensional  linear  randomly  sampled  system.  Namely: 

step(i)  Select  an  appropriate  time  instance  t  >  to  and  a  sampling  interval  A  ,and  calculate  matrices  A(t), 
B{t),  $(t  -(-  A,f)  and  $({  +  &,£). 
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step(ii)  Choose  appropriate  eigenvalues  A ,-'s  and  vectors  £i’s. 

step(iii)  Determine  a  feedback  gain  K  and  matrix  T  using  algorithm  given  in  Section  2.f. 

step(iv)  Determine  matrices  Kp  and  Kv  as  follows. 

For  a  PD  controller  with  a  feedforward  term, 

[KP  Kv)  =  -K 

for  a  computed  torque  controller, 

[Kp  Kv)  =  -Bi(t)K  -  A,(i) 
and  for  a  simple  computed  torque  controller, 

( Kp  Ku]  =  —Bn(t)K 

where  A2(i)  and  B^it)  are  the  lower  half  sub-matrices  of  A(i)  and  B(t),  respectively. 
step(v)  Check  the  stability  using  Props.  6  or  7. 


3.3  Examples 

In  this  section  we  apply  the  result  given  above  to  a  simple  three  dof  robot  “ROBOTEC”  shown  in  Fig.  3.1. 
The  dynamic  equation  is  given  as  follows. 


where 


T  =  M(q)q  +  C{q,q)  +  Bq  +  C{q) 


M 

C 

B 

G 


o-Sj  +  2PS2C3  +  7C3  +  mj  0  0 

0  a  +  m2  3 

0  0S?3  7  +m3 

2{(a.S2  +  0Cs,)C2q2  —  (PS?  +  tC,3)'?393}9i 
—(acS?  +  PSz^CoqP  +  PCozqz" 

{PS2  +  fS3)S3qi  2  +  PCl3q2  " 

61  0  0  1 

0  60  0 

0  0  63  1 

0 

—93S2 

—93C3 


(3.22) 


(3.23) 


Si  =  sin(qi),  Si]  =  sin(qi  +  qj)  and  so  on.  The  Coulomb  friction  is  omitted  here.  Typical  parameters  are 
given  in  Table.  3.1.  Note  the  gear  ratio  is  1  :  8  so  that  the  dynamics  of  actuators  does  not  dominate  the 
robot  dynamics.  Here  we  set  mi,  m2,  m3l  6j , ft2,  and  63  to  be  zero  to  show  the  effect  of  the  dynamic  forces 
more  clearly. 

Now  the  desired  trajectory  was  assumed  to  be  a  straight  line  segment  in  Cartesian  space  connecting 
(10cm,  10cm,  0cm)  at  <0  =  O.Osec  and  (10cm, —10cm,  10cm)  at  tj  =  l.Osec.  The  velocity  and  acceleration 
are  determined  using  5th  order  polynomials.  We  used  t  =  0.5[sec]  and  A  =  10msec,  respectively,  and 
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selected  {0.7,  0.7, 0.7, 0.4, 0.4, 0.4}  and  100[/3  :  7a]  for  eigenvalues  {A,-}  and  [£,],  respectively.  Then  matrices 
Kp  and  Kv  were  calculated  as  follows,  For  the  PD  controller 


'  966.47 

-40.81 

11.54 

46.53 

3.02 

-0.85  ' 

[KP  :  Kv)  = 

33.96 

2300 

310.6 

-3.04 

112.3 

16.72 

x  6.272  x  105 

(3.24) 

-16.84 

305.6 

780.9 

0.83 

16.70 

38.41  _ 

for  the  computed  torque  controller 

1 

'  1520 

-64.18 

18.15 

75.34 

-3.01 

0.91  ' 

II 

£ 

32.91 

1491 

-9.73 

1.50 

73.92 

-0.48 

x  6.272  x  105 

(3.25) 

-36.18 

-28.32 

1492 

-1.78 

-1.47 

73.88  _ 

for  the  simple  computed  torque  controller 

J 

r 

1520 

-64.18 

-18.15 

73.18 

4.76 

-1.34  ‘ 

H 

‘  28.65 

1517 

-11.77 

-2.37 

73.54 

0.43 

x  6.272  x  105. 

(3.26) 

-44.31 

-49.84 

1503 

2.58 

1.19 

73.50 

Figs.  3.2,  3.3  and  3.4  show 

3(<5A)  = 

at  t  =  0.05, 0.25, 0.5, 0.75,  0.95[sec]  for  the  PD  controller,  the  computed  torque  controller  and  the  simple 
computed  torque  controller,  respectively. 

The  following  simulations  were  performed.  At  the  initial  time  t0  =  0,  the  robot  hand  was  located  at 
(11cm,  11cm,  1cm).  Fifteen  streams  of  random  variables  were  used  to  generate  sampling  interval  A's  for 
each  distributions  and  the  worst  examples  are  shown  in  the  figures. 

•  There  is  no  a  and  /?  such  that  $(<,/?)  <  g(t,a)  except  at  t  =  0.5sec  in  Fig.  3.2.  This  means  that 
the  characteristics  of  the  system  varies  so  widely  that  the  PD  controller  may  not  be  able  to  stabilize 
the  system  except  in  the  vicinity  of  t  =  0.5sec  for  any  [a,/?].  In  fact,  the  system  is  not  stable  for 
A  E  U[omsec,  30msec]  although  the  system  is  stable  for  a  constant  sampling  rate  A  =  10msec. 


•  Fig.  3.3  of  g(t,  A)  is  almost  same  for  all  t  for  the  computed  torque  controller.  This  means  that  the 
characteristics  are  almost  same  for  all  t  and  the  system  is  asymptotically  stable  along  the  trajectory 
if  A  E  U[5msec,  30msec],  for  example.  The  simulation  is  shown  in  Fig.  3.5,  where  Ex,  Ey  and 
Ez  axe  the  errors  from  the  desired  trajectory  in  Cartesian  space  for  the  random  sampling  rate  A  E 
U[bmsec,  30msec].  Fig,  3.6  shows  the  simulation  for  A  E  U[5msec]  40msec].  The  system  still 
remains  stable,  but  there  exist  large'  vibrations  around  t  =  0.4sec.  The  system  is  unstable  for 
A  E  U[5msec,  45msec]  for  almost  all  random  streams.  From  Fig.  3.3,  we  have:  (i)  if  ,3  <  33msec, 
there  is  an  a  such  that  the  robot  system  is  asymptotically  stable,  (ii)  if  or  <  21msec,  there  is  a  3  for 
which  the  system  is  asymptotically  stable. 


•  Fig.  3.4  for  the  simple  computed  torque  shows  the  intermediate  properties  of  the  graphics  for  above 
two  controllers.  The  characteristics  change  but  not  so  widely  that  we  can  find  stable  distributions 
for  all  t.  Figs.  3.7  and  3.8  show  the  error  trajectories  for  the  random  rate  A  E  U[5msec,  25msec] 
and  A  E  77(5msec,  40msec],  respectively.  There  are  large  vibrations  around  t  =  0  4sec  in  the  latter 
figure  which  is  almost  same  as  one  for  the  computed  torque  controller.  The  system  is  also  unstable 
for  A  E  U[5msec,  45msec]  for  almost  all  random  streams.  From  Fig.  3.4,  we  have:  (i)  if  3  <  28msec, 
there  is  an  a  such  that  the  robot  system  is  asymptotically  stable,  (ii)  if  a  <  19msec,  there  is  a  3  for 
which  the  system  is  asymptotically  stable. 
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Figure  3.1:  ROBOTEC 


Table  3.2  shows  expectations  and  standard  variations  of  correlation  coefficients  btween  7(ft>  At)  and 
j(h+!,  At+i)  for  k  =  30  and  l  =  1,5,10,20,  where  At  €  U[hmsec,  30msec}.  Five  sets  of  200  samples 
were  calculated  for  each  case.  The  correlation  coefficients  are  all  small  even  for  a  pair  of  7(^30,  A30)  and 
7(<3i,  A31).  Therefore  we  can  assume  that  these  7’s  are  independent  random  variables  one  another. 
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Figure  3.2:  g  x  1000  of  PD  Controller 


deltafsec] 


Table  3.1:  Parameters  of  ROBOTEC 


delta[sec] 


Figure  3.4:  g  x  1000  of  Simple  Computed  Torque  Controller 


timefsec] 

Figure  3.5:  Computed  Torque  Controller  -  £/[5msec,  30msec] 


Table  3.2:  Expectations  and  Standard  Deviations  of  Correlation  Coefficients 


|  £7(f jt ]  =  0.525[sec] 

PD  Controller 

CT  Controller 

SCT  Controller 

1 

Ftyt+zHsec] 

MEAN 

STD 

MEAN 

STD 

MEAN 

STD 

1 

0.543 

0.0271 

0.0829 

-0.0110 

0.0646 

-0.0213 

0.0676 

5 

■iiitm 

0.0519 

■KHI 

0.0340 

0.0780 

0.0295 

0.0772 

10 

0.700 

0.0645 

0.0432 

0.0288 

0.0517 

0.0411 

0.0527 

20 

0.875 

-0.0484 

0.0729 

-0.0727 

0.0650 

-0.0632 

0.0650 
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timefsec] 


Figure  3.6:  Computed  Torque  Controller  -  U[?>msec,  40msec] 


time[sec] 


Figure  3.7:  Simple  Computed  Torque  Controller  -  U[5msec,  25msec] 
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Chapter  4 


Control  of  PUMA  260  under 
Random  Sampling  Intervals 


In  this  chapter,  we  apply  the  results  given  so  far  to  the  control  of  PUMA  260  Robot  Manipulator. 

Corke  [4]  developed  a  new  control  system  MMCS  (Modular  Motor  Control  System)  which  contains  2- 
axis  control  boards  inserted  into  IBM  PC  bus  and  an  adapter  to  connect  to  a  high  performance  workstation 
computer(SUN  3).  The  controller  supports  up  to  16  axes  and  can  be  used  to  control  various  motor  units 
including  robot  manipulators,  hand  systems,  camera  mounts  and  tables.  Control  boards  have  a  common 
clock  and  interrupt  the  host  computer  to  calculate  their  input  signals.  This  facility  makes  it  possible  to  keep 
the  sampling  interval  constant.  The  servo  software  in  the  kernel  of  the  host  computer  supports  position 
feedback  and  velocity  feedback  controls  of  each  joint  with  a  programmable  digital  filter  and  a  Coulomb 
friction  compensator.  However  if  we  wish  to  control  the  torques  of  motors  directly  through  MMCS.  then 
the  control  algorithm  must  be  run  in  a  user  process  and  its  scheduling  cannot  be  guaranteed,  with  possibly 
consequences  for  unstable  and  non-smooth  control,  at  the  present  stage  of  MMCS.  This  is  unavoidable 
under  a  Unix  machine.  Therefore,  in  this  chapter,  we  apply  the  results  given  above  for  random  sampling 
intervals  in  order  to  control  PUMA  260  with  MMCS. 

When  we  discussed  the  control  problems  of  a  two-dimensional  linear  control  system  in  Section  2.5,  we 
expected  that  we  would  be  able  to  apply  the  results  to  robot  manipulators  which  were  controlled  with  a 
randomly  sampled  controller.  In  the  following  ,  first  of  all,  the  distribution  of  sampling  intervals  are  mea¬ 
sured  and  are  approximated  by  a  mixed  uniform  distribution.  Next,  feedback  gains  and  a  transformation 
matrix  are  calculated  for  the  two-dimensional  system  using  the  algorithm  given  in  Section  2.5.  Finally,  the 
stability  is  checked  for  the  case  where  we  use  the  same  feedback  gains  to  control  PUMA  260  by  SUN  3. 
To  do  3o.  linearized  dynamic  equation  of  PUMA  260  is  generated  automatically  by  a  Lisp  program.  Some 
simulation  results  are  also  shown. 


4.1  Control  Scheme  and  Distribution  of  Sampling  Intervals 

If  we  use  a  simple  computed  torque  controller,  then  inputs  ut  are  calculated  by 

ut  =  M(q(tk))f(tk)  +  C(q(tk),q(t  k))  +  B(«(tk))  +  G(q(tfc))  +  CL(q(tk)),  (4.1) 

where  CL{q)  is  a  Coulomb  friction  force  term,  and 

<f  (<t)  =  i(*k)  +  KP(q(tk)  -  q(tk))  +  Kv(i(tk)  -  ?(<*))• 
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Here  the  trajectory  is  assumed  such  that  the  hand  is  directed  to  downward  and  moves  along  a  circle 
with  the  radius  of  10  cm  at  the  center  of  (25  cm,  0  cm,0  cm)  in  the  horizontal  plane  keeping  the  constant 
pose  with  respect  to  the  shoulder  coordinate  system  of  PUMA  260.  It  rounds  the  circle  in  tr  sec.  namely 


Te(t)  = 


0  0  1  25  +  10sin(2xt/tr) 

0  10  10cos(2Tt/tr) 

-10  0  o  ' 


(4.21 


Due  to  the  randomness  of  the  sampling  intervals,  we  cannot  identify  the  current  time  by  counting  the 
numbers  on  iterations  and  we  must  rely  on  function  “clock()”  to  calculate  the  desired  trajectory.  The 
resolution  of  the  function  is  16.666  msec.  Therefore  we  prepared  a  table  of  desired  joint  angles,  angular 
velocities,  and  angular  accelerations  for  each  16.666  msec  from  the  above  trajectory.  This  also  means  that 
we  cannot  measure  the  histogram  of  sampling  intervals  of  Eq.  (4.1)  by  “clockQ”  precisely. 

For  PUMA  260,  we  use  following  control  strategy: 

•  MMCS  interrupts  SUN  3  every  1  msec  and  sends  out  current  commands  to  motor  drivers.  Viscous 
and  Coulomb  friction  are  compensated  in  this  level.  This  also  means  that  the  states  of  joints  (  which 
contain  current  angles  and  angular  velocities  of  them  )  are  updated  each  1  msec. 

•  The  controller  reads  the  current  state  information  from  MMCS,  calculates  Eq.  (4.1),  and  writes  the 
command  values  u(t)  on  MMCS  repeatedly. 

Now  we  assume  that  the  output  of  uclock()”  changes  at  tc0,  tci ,  tc2,  •  •  • ,  where  (tci+i-tcfc  =  16.666msec) 
and  that  the  i-th  sampling  interval  begins  at  f,  6  (<clttcfc+ij,  and  ends  at  t>+1  6  [tCk+n,  tck+n+ 1)  (see  Fig. 
4.1).  Then  the  “clockQ”  changes  n-times  in  the  sampling  interval.  We  define  this  event  as  En,  namely. 


En  :  Event  where  “clockQ”  changes  n-times  in  a  sampling  interval. 

Now  we  assume  that 

•  The  starting  times  of  sampling  intervals  after  the  latest  clock  change  (  y,  =  f,  —  tc*  )  are  subject  to 
uniform  distribution  U[ 0, 16.666  msec j  and  statistically  independent  not  only  of  each  other  but  also 
of  sampling  intervals.  1 


Then  we  have  following  proposition: 

Proposition  8  (Probability  of  Event  En) 

po  =  J*  — /(A)dA, 

and  for  n  >  0 

Pn  —  f  —/(A  +  (n  —  l)/i)dA  +  f  — - — /(A  +  n/j)dA. 

Jo  n  Jo  h 

where  Pn  —  ProAfEnJ.h  =  16.666  msec,  and  /(A)  is  the  probability  density  function  of  sampling  intervals. 
Furthermore  we  have 

n 

£[A]  =  lim  TihP,. 

flSOO  *  * 

1=1 

1  Generally,  this  assumption  is  not  true  for  control  systems.  Usually,  y,  and  y,+ 1  have  a  positive  correlation  if  the  deviation 
of  sampling  intervals  are  not  so  large.  Nevertheless  we  assume  this  because  of  simplicity. 
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Figure  4.1:  Event  En 


<  proof  > 

Here  we  consider  for  the  case  of  n  >  0  only  because  the  case  of  n  =  0  will  be  trivial  from  it. 

We  assume  that  the  i-the  sampling  interval  starts  y  after  the  latest  tck  and  continues  A  seconds.  Then 
it  is  clear  that  if  A  <  (n  -  l)h  or  A  <  (n  +  l)/i  then  we  have  Prob[En]  =  0.  Next,  if  (n  -  l)h  <  A  <  nh 
then  y  >  nh  —  A  must  be  satisfied.  Therefore,  by  the  assumption  on  random  variable  y,  the  probability 
for  the  case  where  sampling  intervals  are  contained  in  (A,  A  +  dA)  is 


A  —  (n  —  1)A 


/(A)rfA. 


On  the  other  hand,  if  nh  <  A  <  (n  +  1)  then  y  must  be  less  than  (n  +  1)A  —  A.  Therefore  we  have 


(n+  l)h  -  A 


/(A)dA, 


for  the  same  small  segment.  Since  these  events  are  exclusive  each  other,  we  have 


fflh 


A  —  (n  —  l)h 


/(A)dA+  f 

Jn 


(n  4-  l)h  -  A 


/(A)dA. 


Substructing  the  integral  variables  appropriately,  we  have  first  half  part  of  the  proposition.  This  equation 
can  also  be  verified  easily  using  characteristic  functions. 

Now  we  have 


hPi  =  /  A/(A)dA+  [  (2h  —  A)/(A)cfA. 

Jo  Jh 


Here  we  assume  next  equation: 


n  rn 

.=1 


r(fl+l)h 

A/(A)dA+  /  n{(n  +  l)h  —  A}/(A)dA, 
Jnh 


42 


then  we  cam  easily  show  that 

rt+l  i»(n+l)A  p(n+2)h 

Y "ihPi=  Af(A)dA  +  (n  +  l){(n  +  2)h-A}f(A)dA. 

Jo  J(n+ l)fi 

Therefore  for  any  n  >  0, 

Jrfih  ^  p(n+l)J* 

I  Af(A)dA  <  V  ihPi  <  /  Af(A)dA. 

o  Jo 

This  inequality  means  the  proposition  is  true.  □ 

The  histogram  of  event  En  were  measured  five  times  for  one  minute  using  SUN  3  and  the  results  are 
shown  in  Table  4.1. 2  We  have  five  independent  conditions  (  P„(n  =  0, 1,2, 3, 4)  ).  Therefore  we  can  use 
five  parameters  to  specify  the  distribution  of  sampling  intervals.  Note  that  the  expectation  of  sampling 
times  is  determined  by  P,  only  and  does  not  depend  on  /( A)  at  all.  This  means  that  we  cannot  use  the 
expectation  to  identify  it. 

Since  we  do  not  any  information  about  the  distribution  of  sampling  intervals,  it  is  reasonable  to  use  a 
distribution  as  simple  as  possible.  Hence  we  assume  it  as  follows: 


(1)  A  =  a  with  probability  p\ . 

(2)  A  is  subject  to 


U[h,2h] 
U[2h,  3 h] 
W(3MA] 


with  probability  p2, 
with  probability  p3, 
with  probability  p*. 


(1)  corresponds  to  the  normal  situation  and  (2)  to  the  case  where  some  delay  occurs  due  to.  for  example, 
interrupt  latencies.  By  applying  Prop.  8  to  this  distribution,  we  have 


Po 

Pi 


hPl 
h  -  a 
h 

P2  +  P3 


P:  +  - 


p  -  cl 
r*  -  T 


!  ~ * 

2 

P3  +  Pa 

\  — 

2 

Pa 

l  — 

2 

l  easily  and  we  have 

Pi 

=  0.918845 

P2 

=  0.080298 

P3 

=  0.000784 

P4 

=  0.000070 

a 

=  2.147  msec 

(4.3) 

(4.4) 

(4.5) 
(4-6) 
(4.7) 


(4.8) 


2 Of  course,  the  distribution  of  sampling  intervals  varies  depending  on  the  number  of  login  users  and  the  quality  of  jobs. 
Therefore  several  data  were  gathered  at  different  chances.  Data  were  very  similar  to  each  other  for  this  system  since  Sun 
3  system  in  GRASP  LAB.  of  Univ.  of  Penn,  is  devoted  to  a  relatively  restrictive  purpose  (to  develop  a  control  system  of 
PUMA  260)  and  only  a  few  people  usually  login  it  . 
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Table  4.1:  Histogram  of  En 


However  there  is  a  problem.  If  we  use  values  given  above,  the  expectation  of  sampling  intervals  becomes 
4.017msec  and  it  is  less  than  the  measured  value  5.34msec.  To  adjust  this  difference,  we  used  a  =  3.587msec 
instead  of  2.147  msec. 

Simulation  were  done  using  this  distribution.  The  results  are  given  in  Table  4.2.  The  mean  and 
probabilities  of  event  £2,  £3,  and  E 4  are  very  near  the  original  data,  while  probabilities  of  Eq  and  E\  are 
different.  This  is  because  of  the  periodicity  of  the  sampling  times.  The  sampling  intervals  have  same  value 
(3.587  msec)  with  probability  0.9  and  y,  has  a  positive  correlation  each  other.  This  distribution  is  still 
useful  since  it  gives  worse  situation  than  the  actual.  Therefore  we  use  this  distribution  to  determine  the 
feedback  gains  of  Eq.  (4.1)  in  the  following  section. 

4.2  Feedback  Gains  and  Stability 

In  order  to  apply  the  results  given  so  far,  we  must  derive  a  linearized  equation  of  PUMA  260  along  a 
reference  trajectory,  which  requires  a  lot  of  times  and  the  task  is  so  cumbersome  and  complicated  that  we 
would  not  be  able  to  do  without  lots  of  mistakes.  To  avoid  this  formidable  business,  a  Lisp  program  is 
developed  which  automatically  generates  a  linearized,  robot  dynamic  equation  in  a  C  program.  The  detail 
is  given  in  Appendix  A. 

4.2.1  PD  Controller 

First  of  all,  the  expectations  of  function  7  were  calculated  under  distribution  (4.8)  for  the  two-dimensional 
system  of  Section  2.5  with  a  PD  controller.  The  results  are  shown  in  Table  4.3.  From  this  table,  we 
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Afmsec 


expectation 


15.0 

-0.0749 

16.0 

-0.0771 

17.0 

-0.0782 

17.5 

-0.0783 

18.0 

-0.0782 

19.0 

-0.0775 

20.0 

-0.0763 

Table  4.3:  A  and  Expectation  of  7  function 


use  A  =  17.5  msec  because  it  gives  the  minimum  expectation  and  we  can  expect  the  fastest  convergence. 
Feedback  gain  matrices  Kp  and  Kv  of  controllers  and  transformation  matrix  T  to  check  the  stability  were 
given  by  Prop.  3  and  Eq.  (2.39)  as  follows: 


Kp  =  Diag{587.76} 

(4.9) 

Kv  =  Diag  {46.29} 

(4.10) 

T  — 

'  Diag{— 0.0133} 

Diag{-0.0165} 

(4.11) 

1  — 

Diag{0.651} 

Diag{0.333} 

where  Diag{x}  is  a  six-dimensional  diagonal  matrix  with  the  same  diagonal  element  x.  Figs.  4.2.  4.3.  and 
4.4  show  7  functions  of  PUMA  260  calculated  at  the  positions  of  0,72, 144,216.  and  288  deg  for  0.2. 0.5 
and  1  rps.  The  dot  line  shows  7  function  for  the  two-dimensional  system.  7  functions  of  PUMA  260  are 
very  similar  to  each  other  in  spite  of  the  difference  of  the  position  and  velocity.  It  is  also  shown  that  the 
expectation  of  7  functions  for  PUMA  260  will  be  larger  a  little  than  that  of  the  two-dimensional  system, 
but  it  may  be  still  negative.  In  fact,  Table  4.4  shows  that  the  expectations  of  7  function  for  PUMA  260 
are  negative  for  all  positions  and  velocities.  Therefore  PUMA  260  is  asymptotically  stable  with  probability 
one. 

Figs.  4.5  —  4.8  show  simulation  results  for  Stream  3  in  Table  4.2.  Fig.  4.5  shows  the  desired  trajectory 
for  the  case  of  0.2  rps.  Figs.  4.6,  4.7,  and  4.8  show  the  error  angles  in  degree  for  each  joint  for  the  cases 
of  0.2,  0.5,  and  1  rps,  respectively.  The  mean  of  the  absolute  values  of  errors  and  the  maximum  for  one 
minute  are  listed  in  Table  4.5.  The  errors  become  larger  according  to  the  increase  of  velocity.  Figs.  4.9 
and  4.10  show  the  trajectories  in  x-y  plane  and  x-z  plane  for  the  case  of  1  rps,  respectively.  The  maximum 
errors  in  the  radius  and  in  the  z-coordinates  are  listed  in  Table  4.6.  This  implies  that  the  errors  in  joint 
angles  generate  not  the  deviation  across  the  desired  trajectory  but  the  delay  along  it.  The  main  cause  of 
this  is,  of  course,  large  delays  in  sampling  times  and  another  is  the  rough  resolution  of  “clockQ”  function. 
If  PUMA  260  travels  along  the  circle  at  lrps,  then  it  moves  about  six  degrees  during  16.666  msec,  which 
corresponds  to  about  four  degrees  of  Joint  I  at  the  largest  value. 

4.2.2  PID  Controller 

PID  controllers  make  robot  manipulator  robust  for  slow  changes  of  system  parameters.  Therefore  we  apply 
the  results  given  in  Subsection  2.6.1  to  design  a  PID  controller  for  PUMA  260.  Namely,  we  use  following 
controller: 


Ut  =  MWk)W(tk)  +  C(q{tk),  ?-(<*))  +  fl(fftO)  +  G(q(tk))  +  CL(q(tk)).  (4.12) 
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Table  4.4:  Expectation  of  7  function  for  PUMA  260 


Table  4.5:  Mean  and  Maximum  of  Joint  Angle  Errors 


Maximum  Error 

RPS 

Z-Coordinate(cm] 

mm 

EH 

1.0 

Table  4.6:  Maximum  Error  in  Cartesian  Space  for  PD  Controller 
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Figure  4.6:  Joint  Angle  Errors  for  0.2  rps 


Errors 

[deg] 


timefsec] 


Figure  4.7:  Joint  Angle  Errors  for  0.5  rps 
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Figure  4.10:  Trajectory  in  x-z  Plane  for  1.0  rps 


Table  4.7:  Maximum  Error  in  Cartesian  Space  for  PID  Controller 


where 

s*+i  =  *fc+(«(*fc)-«ttk)).  (4.13) 

<f(*t)  =  'q(tk)  +  Kp(q(tk)  -  q(tk))  +  Kv(q(tk)  -q(tk))  +  Ki r*. 

Using  the  same  procedure  as  we  used  for  PD  controllers,  we  have  A  =  13  msec, /\p  =  Dia<7{1307.69},  Kv  = 
Diag{ 64.62},  and  K,  =  Dia^{53.25}.  Figs.  4.11  ~  4.14  show  simulation  results  for  five  seconds,  where 
the  gravity  compensation  term  G{q{tk))  was  removed  from  Eq.  4.12.  Figs.  4.11  and  4.12  are  for  the  PD 
controller  given  above  and  the  others  for  the  PID  controller  designed  here.  The  velocity  is  1  rps  for  both 
controllers.  For  the  PD  controller,  PUMA  260  falls  about  0.7  cm  at  the  most  stretched  position  in  Fig. 
4.11,  while  the  maximum  error  in  the  radius  is  about  0.6  cm  (  Fig.  4.12  ).  If  we  use  the  PID  controller, 
the  vertical  deviation  becomes  less  than  0.2  cm  at  the  worst  (  Fig.  4.13  )  and  the  maximum  radius  error 
becomes  a  little  larger  (  Fig.  4.14  ).  Table  4.7  shows  the  errors  in  the  radius  and  the  z-coordinate  for 
0.2, 0.5,  and  1.0  rps  from  t  =  0.5  sec  to  5.0  sec. 
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Figure  4.13:  Trajectory  in  x-y  Plane  for  PID  Controller 
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Figure  4.14:  Trajectory  in  x-z  Plane  for  PID  Controller 
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Chapter  5 

Conclusions 


In  this  report,  the  stability  of  randomly  sampled  linear  control  systems  was  discussed  and  the  following 
results  were  obtained. 

•  A  necessary  and  sufficient  condition  for  the  asymptotical  stability  with  probability  one  was  obtained 
for  one-dimensional  systems. 

•  Applying  the  results  to  the  systems  whose  sampling  intervals  are  subject  to  Bernoulli  distribution, 
uniform  distribution  and  coupled  uniform  distribution,  the  stability  conditions  were  derived  explicitly. 

•  A  sufficient  condition  for  the  asymptotical  stability  with  probability  one  was  obtained  for  multi¬ 
dimensional  systems. 

*  *  i 

«  For  a  typical  two-dimensional  system,  a  concrete  design  procedure  shown  for  a  PID  controller  and  a 
PD  controller  with  one  step  delay  as  well  as  a  PD  controller. 

•  The  above  results  were  applied  to  robot  control  systems  and  the  effectiveness  of  the  PD  controller 
with  a  feedforward  term,  computed  torque  controller,  and  simple  computed  torque  controller  were 
compared  with  a  random  sampling  rate.  It  was  shown  that  PD  controllers  are  very  sensitive  to  the 
randomness  of  the  sampling  rate,  while  computed  torque  controllers  and  simple  computed  torque 
controllers  are  useful  for  random  sampling  rates  which  have  fairly  wide  distributions. 

•  Finally,  the  stability  problem  of  PUMA  260  controlled  by  work-station  SUN  3  was  discussed.  First 
of  all,  the  distribution  of  sampling  intervals  was  measured  and  approximated  by  a  mixed  uniform 
distribution.  Next,  the  feedback  gains  aind  the  transformation  matrix  were  obtained  for  the  two 
dimensional  system  mentioned  above.  Then  the  stability  of  PUMA  260  with  same  gains  were  checked 
using  same  transformation  matrix.  To  do  so,  a  Lisp  program  which  generates  a  linerized  equations 
along  a  reference  trajectory  automatically  was  developed.  Finally  some  simulation  results  were  shown. 
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Appendix  A 

Linearized  Dynamic  Equation  of 
Robot  Manipulators 


Here  we  develop  a  Lisp  program  to  generate  a  C  program  which  calculates  linearized  dynamic  equations 
of  manipulators  along  a  given  trajectory  recursively. 

In  general,  dynamic  equations  of  robot  manipulators  are  very  complicated  and  to  linearize  the  equations 
is  a  formidably  burdensome  work,  while  we  often  need  tinearized  equations  to  assure  the  stability  of  the 
systems  and  so  on.  One  way  to  deal  with  this  work  is  to  develop  a  lisp  program  to  write  the  equations  for 
us. 

Since  we  have  already  had  efficient  recursive  algorithms  to  calculate  the  actuator  torques( forces)  of 
robot  manipulators  for  given  configuration  (for  example  [6]),  we  can  easily  deduce  recursive  equations  to 
calculate  small  deviations  of  the  torques(forces)  given  small  deviations  of  joint  variables.  Since,  of  course, 
linearized  equations  are  linear  with  respect  to  joint  variables,  we  can  calculate  the  coefficients  from  the  i-th 
variable  to  the  torques(forces)  by  setting  the  i-th  variable  to  be  equal  to  one  and  the  others  to  be  equal  to 
zero.  But  if  we  use  the  general  equations  directly,  the  amount  of  the  calculation  becomes  again  formidable. 
Fortunately,  the  parameters  of  the  equations  contain  a  lot  of  zeros,  ones,  minus-ones,  and  common  terms 
which  appears  many  times  during  the  calculation.  It  is  also  known  that  there  is  a  linear  dependence  among 
parameters  and  the  amount  of  the  calculation  can  be  reduced  by  using  the  dependence.  We  can  use  these 
facts  if  we  develop  a  Lisp  program  to  generate  a  C  program  for  it. 

Dynamic  equations  of  manipulators  are  described  as  follows: 


r  =  R{q,  q,  q),  ,  (A.l) 

=  A/(q)q  +  C(q,  q)  +  <7(q),  (A.2) 

where  q,  q,  q  are  angle(position),  velocity,  and  acceleration  vectors  of  joint  variables,  respectively.  M  is 

the  inertia  matrix,  C  the  centrifugal  and  Coriolis  force,  and  G  the  gravitational  force.  Here  we  neglect  the 
Coulomb  friction  since  it  does  not  appear  in  linearized  equations. 

We  assume  that  a  desired  trajectory  {q.fj,  f}  is  given  where 

f=K(q,q,q),  (A.3) 

and  define  small  deviation  vectors  <5q,<5q,<5q,  and  Sr  as  follows: 

<5q  =  q-q>  (a.4) 
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Then  we  have 


,5q  =  q-4, 

Sq  =  q-4, 

St  =  r  —  f. 

Sr  =  P5q  +  QSq  +  776q, 

where 

P  =  iVf(q), 

Q  =  [dP/dq](q,  q), 

R  =  [<9i7/dq](q,q,q). 

Linearized  equation  is  given  as  follows: 

Sx(t)  =  A(t)Sx(t)  +  B(t)6T, 

where 


Ir 

P~lR  ' 


Recursive  equations  of  dynamics  of  the  manipulator  are  given  as  follows: 
(Forward  Equations] 

wf  =  P((w,_i  +  ?>), 

Wj  =  +  ?,Z  +  ^Wi.i  x  z), 

v<  =  Rifw,-!  X  Pi_!  +  Wi_i  X  (w,_!  X  Pi-!)  +  Vf-l], 

[Backward  Equations] 


F< 

=  mi(vi  +  Wi  x  (pc  +  Si)  +  w,-  x  (w,  x  (pi  +  s,-))], 

(A. 16) 

Ni 

=  7,Wi  +  w,-  x  (7,-Wj), 

(A. 17) 

f; 

=  77,+ifi+i  +  F{, 

(A. 18) 

Hi 

=  72,>i[n,-+i  +  (P(+iPi)  x  fi+1]  +  m,(pi  +  s ,)  x  vf  +  /V,-, 

(A- 19) 

A 

=  n‘(7Zjz)  +  7  Af'q,-  +  77  A 

(A.20) 

where  qt  and  q{  are  the  velocity  and  the  acceleration  of  the  joint  variable  of  link  i.  w,-,  w,,  and  v,  are  the 
angular  velocity,  angular  and  linear  acceleration  of  the  frame  i  with  respect  to  the  frame  i,  respectively. 
Fi,  jV,-,  and  n;  are  the  force  due  to  the  motion  of  link  «,  the  torque  due  to  the  motion  of  link  i,  the  total 
force  in  the  link  i,  and  the  total  torque  in  the  link  i  expressed  with  respect  to  the  frame  i,  respectively. 
Pi  is  the  vector  from  the  origin  of  frame  i  —  1  to  the  origin  of  frame  i  and  s,-  the  vector  from  the  origin  of 
frame  i  to  the  gravity  center  of  link  1  with  respect  to  frame  i.  m;  is  the  mass  of  link  i  and  7;  is  the  inertia 
matrix  of  link  i  at  the  origin  of  frame  i  with  respect  to  frame  i.  Ri  is  the  rotation  matrix  from  frame 


(A. 13) 
(A. 14) 
(A.  15) 


Sx  = 

A(t)  = 
B(t)  = 


q-q 

?-?  J 

0 

-P~lQ 

0 

p-i 


(A.5) 

(A.6) 

(A.7) 

(A. 8) 


(A. 9) 
(A. 10) 
(A. 11) 


(A. 12) 
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t  —  1  to  i.  IA{  and  RD{  are  inertia  and  velocity  friction  parameters  of  the  i~th  actuator,  respectively,  and 
*  =  (0,0,1)*.  ' 

Then  the  differential  equations  are  given  as  follows: 


[Forward 

Equations] 

5w, 

= 

R[(<5w,-_i  +  Sq{z )  +  SqiEiWi, 

(A. 21) 

6w, 

= 

R{(Sw{^i  +  6q\z  +  %w,_i  x  z  +  fjiw,.!  X  z)  + 

(A. 22) 

<w, 

— 

X  P,_I  +  8  Wf_!  X  (w;.!  X  Pi_i)  +  W,  _  !  X  (iw.-.j  X  p.-i)  +  <5v,-_i] 

+6qiEiVi, 

(A. 23) 

[Backward  Equations] 

SF 

— 

m,[5v,-  +  Swj  x  (p,-  +  s,-)  +  <5w<  x  (w(  x  (p,-  +  s,-))  +  wf  x  (5w,-  x  (pf  +  s,))], 

(A. 24) 

SN 

— 

7,-5wi  +  5w,-  x  (7,-w,-)  +  wi  x  (7,'5w,), 

(A. 25) 

Sf 

= 

7f,+i^f,+i  4-  SF{  +  Sqi+iDRi+iii+x, 

(A. 26) 

6n 

— 

7f,+i[5n,+i  4-  (7i,'+iPi)  x  5fi+i  +  ^9i+i(£f+i7?,'+iP,)  x  f<+1]  + 
$gi+l07?i+I[n{+i  4-  (7?,-+1p,)  x  fi+1]  +  m,(pi  +  sf)  x  <5v,-  +  SNit 

(A. 27) 

Srf  = 

5n[(72,-z)  +  8qi-n.\{EiR\z)  +  7A,%  4  RDiSqi, 

(A. 28) 

where  dtU/dqi  =  DRi  and  dR\/dqi  —  EiR\. 

If  we  use  5q  =  0,  <5q  =  0,  and  5q  =  0  except  Sqj  —  1  for  some  j,  then  5r,-  gives  c,j  element  of  C 
matrix.  Therefore,  if  we  use  the  equations  given  above  directly  to  calculate  matrices  A,B,  and  C  of  the 
linearized  equation  at  {q(<)i  q(0>  A(0>  ^(0}  for  some  t,  then  we  must  calculate  the  dynamic  equation  once 
and  differential  equations  18  times. 

The  Lisp  program  developed  here  takes  the  symmetry  property  of  A  matrix  and  the  linearly  dependence 
of  parameters  of  PUMA260  into  the  consideration.  Alberto  et  al.  [9]  investigated  the  dependence  among 
parameters  of  PUMA260  and  showed  only  23  parameters  are  significant  which  contain  6  actuator  inertias, 
6  velocity  frictions,  and  6  Coulomb  frictions. 

The  Lisp  program  generates  five  procedures  as  follows: 

D_equation(<?,  dq,  ddq,  tau)  calculates  input  r  from  (q,dq,ddq). 

comm-terms(q,dq,ddq,taxi)  calculates  35  common  terms  used  to  calculate  matrices  P,  Q,  and  R. 
P_matrix(F)  calculates  P  matrix. 

Q_matrix(dq,Q)  calculates  Q  matrix.  .  1 

Rjnatrix(dq,R)  calculates  R  matrix. 

Table  A.l  shows  the  number  of  operations  contained  in  the  C  program  generated  by  the  Lisp  program. 
Linearized  equation  is  calculated  by  these  procedures  as  follows: 

void  Hatr icoa ( q , dq, ddq , tau , P , Q , R) 

real  q[mra]  ,dq[mm]  , ddq  [nun]  ,tau[mra]  ,P[mm]  ,Q[mn]  ,  R  Darn]  ; 

{ 

D.equationCq.dq.ddq, tau) ; 
coronLtennsCq.dq.ddq.tau) ; 

P-matrix(P) ; 
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Q.matrix(dq, Q) ; 

R-matrix(dq,R) ; 

} 

void  L_Equation(q,dq,ddq, A,B) 

real  q[mm]  ,dq[mra]  ,ddq[mra]  ,A[mm]  [mm]  ,  8  Cmm]  [mm]  ; 

{ 

real  AA[mm] [mm] ,tau[min] ,P[mm] [mm] ,Q[mm] [ram] ,RCmm] [mm] ,det,eps=l .OE-6; 
int  rank ,  i ,  j  ; 

Matrices (q.dq.ddq.tau.P.Q.R) ; 

Mat_diag(B ,6, 1.0); 

Hat-sweep(P,B,6 , 6 ,6, ftdet , eps, 4rank) ; 

Mat-mul (B , R , AA , 6 , 6 , 6) ; 

ior(i=0;i<6;i++)ior(j=0; j<6 ; j++)  A[i][j]  =  (-AA[i][j]); 
Mat_mul(B,Q,AA,6,6t6)  ; 

ior(i=0;i<6;i++)for(j=0;  j<6;  j++)  A[i]  [j+6]  =  (-AA[i][j]); 

} 


where  matrices  A  and  B  are  6  x  12  and  6x6  dimensional  matrices  and  correspond  to  lower  half  sub¬ 
matrices  of  A  and  B  in  Eq.  (A.  12),  respectively.  The  dynamic  equation  which  calculates  q  from  q,q,  and 
r  is  given  by 

void  Dynamics(tau,q, dq.ddq) 
real  tauQnm]  ,q[mm]  ,dq[mm]  ,ddq[mm]  ; 

{ 

real  tauOQnm] ,P [mm] [nun] , A A [mm] [mm] ,  det , eps=l .OE-6 ,dql [mm] ,ddql [mm] , taul [mm] ; 
int  i , j .rank; 

*or(i=0;i<6;i++)  ddq[i]=0.0; 

D-aquationfq.dq.ddq.tauO) ; 
comm-termsfq.dq.ddq,  tauO) ; 

P-matrix(P) ; 

ior(i=0 ;  i<6 ;  i++)  AA[i]  [0]=tau[i]-tau0[i]  ; 

Mat_sweep(P, AA ,6 , 6, 1 ,idet , eps ,&rank) ; 

lor(i=0;i<6;i++)  ddq[i]+=AA[i] [0] ; 

} 
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Table  A.l:  Number  of  Operations 
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♦include  <raath.h> 


♦define 

real 

float 

♦define 

mm 

6 

♦define 

G  9. 

81 

♦define 

M6  2 

.768114 

♦define 

Z6  0 

.01401 

♦define 

Y4  { 

-0.38219) 

♦define 

AD  4 

0.079781 

♦define 

CD  4 

0.077761 

♦define 

IA1 

0.091631 

♦define 

IA2 

0.136312 

♦define 

IA3 

0.030843 

♦define 

IA4 

0.001781 

♦define 

IA5 

0.006759 

♦define 

IA6 

0.001262 

♦define 

RD1 

0.575662 

♦define 

RD2 

0.94467 

♦define 

RD3 

0.417502 

♦define 

RD4 

0.066791 

♦define 

RD5 

0.101721 

♦define 

RD6 

0.030363 

♦define 

A2  0 

.2032 

♦define 

D3  0 

.12624 

♦define 

D4  0 

.2032 

static 

static 

static 

3tatic 

static 


real  q[mm]  ,  dq[mm]  ,  ddq[mm]  ,  tau  [mm]  ,  P  [nun]  [mm]  ,  Q  [mm]  [mm]  ,  R  [mm]  [mm] 
real  dl_Wx [ 6 ] , dl_Wy [ 6 ] , dl_Wz [ 6 ]  ,  dl_dWx [ 6 ] , dl_dWy [ 6 ] , dl  dWz [  6  ]  ; 
real  dl_dVx[6] ,dl_dVy[6] ,dl_dVz[6] ,dl_Fx[S] ,dl_Fy[6] ,dl_Fz[6] ; 
real  dl_Nx[6] , dl_Ny [6] , dl_Nz [6] , dl_fx [ 6] ,dl_fy[6] ,dl_fz[6] ; 
real  dl_nx [ 6] , dl_ny [ 6 ] , dl_nz [ 6] ; 


static  real 
static  real 
static  real 
static  real 
static  real 
static  real 
static  real 
static  real 
static  real 
static  real 
static  real 
3tatic  real 
static  real 


S1,C1,S2,C2,S3,C3,S4,C4,S5,C5? 

S6,  C6, Wx2, Wy2, dWx2,  dWy2, dVx2, dVy2  r  Wx3, Wy3; 

Wz3 , dWx3, dWy3, dWz3 , dVx3, dVy3 , dVz3, Wx4 , Wy4 , Wz4  ; 
dWx4 , dWy4 , dWz4 , dVx4 , dVy4 , dVz4 , Wx5 , Wy 5 , Wz5 , dWx5 ; 
dWy5, dWz5, dVx5, dVy5, dVz5, Wx6, Wy6, Wz6, dWx6, dWy6; 
dWz6, dVx6, dVy6, Fx6, Fy6, Fz6, nx6 , ny6, fx5,  fy5; 
nx5, ny5 , Fx4 , Fy4 , Fz4 , fx4 , fy4 ,  f z4 , Nx4 , Ny4 ; 

Nz4 , nx4 , ny4 , nz4 , fx3, fy3 , nx3, ny3, nz3 , fx2; 
fy2 ,  f  z2 ,  nx2 ,  ny2 ,  nz2 ,  CMO ,  CM1 ,  CM2 ,  CM3  *  CM 4  ; 

CM5 ,  CM6 ,  CM7 ,  CM8 ,  CM9 ,  CM1 0 ,  CM1 1 ,  CM1 2 ,  CM1 3 ,  CM1 4 
CM1 5 , CM1 6 , CM1 7 , CM1 8 , CM1 9 , CM2  0 , CM2 1 , CM2  2 , CM2  3 , QM2  4 ; 
CM2  5 , CM2  6 , CM2  7 , CM2  8 , CM2  9 , CM3  0 , CM3 1 , CM3  2 , CM3  3 , CM3  4 ; 
CM35; 


void  D_equation <q, dq, ddq, tau) 

real  q[mm] , dq [mm] , ddq [mm] , tau [mm]  ; 

( 


real  TMO , TM1 , TM2 , TM3 , TM4 , TM5 , TM6 , TM7 , TM8 , TM9 ; 

51-  sin (q [ 0 ] ) ; 

Cl-  cos (q[0] ) ; 

52-  sin ( q [ 1 ] ) ; 

C2-  cos (q(l] ) ; 

53-  sin (q[2] ) ; 

C3-  cos (q [ 2 ] ) ; 
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C4-  cos (q[3] ) ; 

S5-  sin  (q[4] ) ; 
C5-  cos ( q  C  4 ] ) ; 

S6»  sin (q C 5 ] ) ; 
C6-  cos (q[5] ) ; 


Wx2«  S2*dq[0] ; 

Wy2~  C2*dq[0]; 

TM0-  dq[0I *dq[l] ; 
dWx2-  S2*ddq[0]+C2*TM0; 
dWy2«  C2*ddq[0] -S2*TM0 ; 

dVx2-  a2*G; 
dVy2-  C2*G; 


Wx3=  S3*Wy2+C3*Wx2; 

Wy3-  - (dq[l] +dq[2] )  ; 
Wz3»  C3*Wy2-S3*Wx2; 

TM0-  Wx2*dq[2] ; 

TM1-  Wy2*dq[2] ; 

TM2-  dWy2-TM0; 

TM3-  dWx2+TMl; ' 
dWx3-  S3*TM2+C3*TM3; 
dWy3»  -  (ddq[2] +ddq(  1  ]  ) .; 
dWz3-  C3*TM2-S3*TM3; 

TM0-  ddq[l]*A2; 

TM1-  dq [ 1 ] * A2 ; 

TM2-  Wy2*A2; 

TM3-  Wx2*TM2; 

TM4-  Wy2*TM2; 

TM5-  dq [ 1 ] *TM1 ; 

TM6-  TM3+TM0; 

TM7-  TM6+dVy2; 

TM8-  TM4+TM5 ; 

TM9-  dVx2-TM8; 
dVx3-  S3*TM7+C3*TM9; 
dVy3-  dWy2*A2-Wx2*TMl ; 
dVz3-  C3*TM7-S3*TM9; 


Wx4-  S4*Wy3+C4*Wx3; 
Wy4-  Wz3+dq[3]; 

Wz4-  S4*Wx3-C4*Wy3; 

TM0-  Wy3*dq[3] ; 

TM1-  Wx3*dq[3] ; 

TM2-  dWx3+TM0; 

TM3-  dWy3-TMl; 
dWx4«  S4*TM3+C4*TM2; 
dWy4-  ddq [ 3 ] +dWz3 ; 
dWz4-  S4*TM2-C4*TM3; 


TM0-  dWz3*D3; 
TM1-  Wz3*D3; 
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TM2-  Wx3*D3; 

TM3-  Wy3*TM2; 

TM4-  Wz3*TMl; 

TM5-  Wx3*TM2 ; 

TM6-  TM0-TM3; 

TM7-  TM6+dVx3; 

TM8-  TM4+TM5; 

TM9-  TM8+dVy3; 
dVx4-  S4*TM9+C4*TM7; 
dVy4-  dVz3- (Wy3*TMl+dWx3*D3) ; 
dVz4«  S4*TM7-C4*TM9; 


Wx5-  S5*Wy4+C5*Wx4; 

Wy5«  - (Wz4+dq[4] ) ; 

Wz5«  C5*Wy4-S5*Wx4; 

TMO-  Wx4 *dq [ 4 ] ; 

TM1-  Wy4*dq[4 ] ; 

TM2-  dWy4 -TM0 ; 

TM3-  dWx4+TMl ; 
dWx5»  S5*TM2+C5*TM3; 
dWy5-  - (ddq[4] +dWz4) ; 
dWz5=  C5*TM2-S5*TM3; 

TMO-  dWz4*D4 ; 

TM1-  Wz4*D4; 

TM2-  Wx4*D4 ; 

TM3-  Wz4*TMl; 

TM4=  Wx4  *TM2 ; 

TM5=  Wy 4  *  TM2 ; 

TM6i=  TM5-TM0; 

TM7  =  TM3+TM4; 

TM8*  TM6+dVx4; 

TM9-  dVy4-TM7; 

dVx5=  S5*TM9+C5*TM8; 

dVy5-  - (Wy4*TMl+dWx4*D4+dVz4 ) ; 

dVzS-  C5*TM9-S5*TM8; 


Wx6»  S6*Wy5+C6*Wx5; 
Wy6-  C6*Wy5-S6*Wx5; 

Wz6  =  Wz5+dq[5]; 

TMO*  Wx5*dq[5]; 

TM1-  Wy5*dq[5]  ; 

TM2-  dWy5-TM0; 

TM3-  dWx5+TMl; 
dWx6=  S6*TM2+C6*TM3; 
dWy6-  C6*TM2-S6*TM3; 
dWz6-  ddq(5J+dWz5; 

dVx6-  S6 *,dVy5+C6 *dVx5 ; 
dVy6»  C 6 * dVy 5 - S 6 * dVx 5 ; 


/.«***  backwrad  equation  *»**/ 

TMO-  Wx6*Z6; 

TM1-  Wy6*Z6; 

Fx6-  M6*dVx6+dWy6*Z6+Wz6‘TM0; 
Fy6-  M6*dVy6+Wz6*TMl-dWx6»Z6; 
Fz6-  M6*dVz5- (Wx6*TM0+Wy6*TMl) ; 
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nx6-  - (26*dVy6) ; 
ny6-  Z6*dVx6; 

tau [5] -  RD6*dq [ 5 ] +IA6*ddq [ 5] ; 

fx5-  C6*Fx6-S6*Fy6; 
fy5-  C6*Fy6+S6*Fx6; 

nx5«  C6*nx6-S6*ny6; 
ny5-  C6*ny6+S6*nx6; 

tau [4 ] -  RD5*dq[4] +IA5*ddq[4] -ny5; 

TM0-  Wz4*Y4; 

TM1-  Wx4*Y4; 

Fx4»  Wy4*TMl -dWz4*Y4 ; 

Fy4«  -(Wz4*TM0+Wx4*TMl) ; 

Fz4-  dWx4*Y4+Wy4*TM0; 

fx4»  C5*fx5-S5*Fz6+Fx4; 
fy4-  C5*Fz6+S5*fx5+Fy4 ; 
fz4-  Fz4-fy5; 

TM0=  AD4*Wx4 ; 

TM1-  CD4*Wz4 ; 

Nx4=  Wy4*TMl+AD4*dWx4; 

Ny4-  Wz4  *TM0-Wx4  *TM1 ; 

Nz4-  C34*dWz4-Wy4*TM0; 

TM0=»  SS*D4  ; 

TM1  =  C5  *D4 ; 

TM2-  TM0*fy5; 

TM3-  TMl*fy5; 

IH-  nx5-TM3; 

nx4«  Y4*dVz4+C5*TM4-S5*TM2+Nx4; 

ny4  =  C5*TM2+S5*TM4+Ny4; 

nz4»  Nz4- (Y4*dVx4+TMl*fx5-TM0*Fz6+ny5) 

tau [ 3] *  ny4+RD4*dq [3 ] +IA4*ddq [ 3 ] ; 

fx3*=  S4*f z4+C4*fx4 ; 
fy3»  S4*fx4-C4*fz4; 

TMO-  C4*D3; 

TM1-  S  4  *  D  3  ; 

TM2-  TMO  *  fy4  ; 

TM3-  TMl*fy4; 

TM4-  nx4 -TM2 ; 

TM5=  nz4 -TM3 ; 
nx3-  S4  *TM5+C4  *TM4 ; 
ny3-  S4*TM4-C4*TM5; 
nz3»  TMO  *  f X4+TM1 *  f  z4+ny4 ; 

tau [2] «  RD3*dq[2 ] +IA3*ddq[2  J -ny3; 

fx2«  C3*fx3-S3*fy4; 
fy 2-  C3*fy4+S3*fx3; 
fz2-  - ( fy3) ; 

TMO-  C3  *A2 ; 

TM1-  S3*A2; 

TM2-  TMO  *  fy3 ; 

TM3—  TMl»fy3; 

TM4-  TM2+nz3; 
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TM5-  TM3+nx3; 
nx2-  C3*TM5-S3*TM4; 
ny2-  C3*TM4+S3*TM5; 
nz2-  TM1 *  f x3+TM0  *  f y 4 -ny 3 ; 

tau [1 ] *  nz2+RD2*dq[l] +IA2*ddq[l] ; 

tau[0] -  C2*ny2+S2*nx2+RDl*dq[ 0 ] +IAl*ddq [ 0 ] ; 


1 


Additions 

73 

Substractions 

53 

Multiplications 

184 

void  conra_terms (q, dq, ddq, tau) 

real  q[mm] ,  dq[mm]  ,  ddq[nan] ,  tau  [ram]  ; 

( 


CM0-  C5*$4; 

CM1-  AD  4  *  S  4 ; 

CM2-  CD4*C4; 

CM3-  C4*Y4; 

CM4-  S4*Y4 ; 

CMS-  C6*C4; 

CM6-  S6*C4 ; 

CM7-  C4*dq[5] ; 
CM8-  C4*D4; 

CM9-  S4*D4 ; 

CM10—  S4  *dq( 4 ] ; 
CM11-  S3*A2 ; 
CM12-  C3*A2; 
CM13-  C4*D3; 
CM14-  S4*D3; 
CM15-  C5*D4 ;  . 
CM16-  S  5  *  D  4 ; 
CM17-  AD4  *Wx4 ; 
CM18-  CD4 *Wz4 ; 
CM19-  W  z  4  *  Y  4  ; 
CM20-  Wx4*Y4; 
CM21-  Wy6*26; 
CM22-  Wx  6  *  Z  6 ; 

CM2 3-  Wz4*D4; 

CM2 4-  Wx4  *D4 ; 

CM2 5-  Wz3*D3; 

CM2 6-  Wx3*D3; 
CM27-  Wy2*A2; 

CM2 8-  Wx4*CM9; 
CM2 9-  Wz4*CM8; 
CM30-  S4*CM24; 
CM31-  C4 *CM23 ; 
CM32-  Wy4 *CM9; 
CM3 3-  CM2 8 -CM2 9; 
CM3 4-  CM3 0 -CM3 1 ; 
CM3  5-  CM3  3 -t- CM3  4; 
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void  R_matrixl (dq, R) 

real  dq[mm],  R[mm] [mm] ; 

{ 


R [ 5 ] [0]-  0.0 
R[4] [0]-  0.0 
R[3] [0]-  0.0 
R [2 ] [0]-  0.0 
R[1 ] [0]-  0.0 
R [0 ] [0]«  0.0 


} 

void  R_matrix2 (dq, R) 

real  dq[mm],  R[mm] [mm] ; 

{ 


real  TMO ,  TM1 , TM2 , TM3 , TM4 , TM5 , TM6, TM7 , TM8 , TM9 ; 
real  TM1 0 , TM1 1 , TM12 , TM1 3 , TM1 4 , TM1 5 ; 

dl_Wx [  2  ]  -  C3*Wy2-S3*Wx2; 
dl_Wz[2]«  - (C3*Wx2+S3*Wy2) ; 

TMO-  Wy2*dq[2]; 

TM1-  Wx2*dq[2 ]'; 

TM2-  dWx2+TM0; 

TM3-  dWy2 -TM1 ; 

dl_dWx [ 2 ] -  C3*TM3-S3*TM2; 

dl_dWz [2 ] -  - (C3*TM2+S3*TM3) ; 

TMO-  Wx2*A2; 

TM1-  Wy2*CM27; 

TM2-  Wx2  *  TMO ; 

TM3-  Wy2*TM0 ; 

TM4—  Wx2  *CM27 ; 

TM5-  TM1-TM2; 

TM6-  TM3+TM4; 

TM7-  TM5-dVx2; 

TM8-  TM6+dVy2 ; 

dl_dVx [2 ] -  S3*TM7+C3*TM8; 

dl_dvy [ 2 ] -  - (Wy2*dq [ 1 ] »A2+dWx2 * A2 ) ; 

dl_dVz [2] -  C3*TM7-S3*TM8; 

dl_Wx[3]-  C4*dl_Wx[2]; 
dl_Wz [ 3 ] -  S4*dl_Wx(2] ; 

TMO-  dl_Wx [2 ] *  dq [ 3  ]  ; 

dl_dWx [ 3 ] -  C4*dl_dWx[2] -S4*TM0; 

dl_dWz [3] =  C4*TM0+S4*dl_dWx [2] ; 

TMO-  dl_dWz [2] *D3; 

TM1-  dl_Wz(2I*D3; 

TM2-  dl_Wx[2)*D3; 

TM3-  Wy3*TM2; 

TM4-  Wz3*TMl; 
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TM5-  Wx3*TM2; 

TM6-  dl_Wz [2] *CM25; 

TM7-  dl_Wx [ 2 ] *CM2 6 ; 

TM8-  TM0-TM3; 

TM9-  TM8  +dl_dVx [ 2 ] ; 

TM10-  TM4+TM5; 

TM11-  TM6+TM7; 

TM12-  TM10+TM11; 

TM13-  TM1 2+dl_dVy [2] ; 
dl  dVx [ 3 ] —  S4*TM13+C4*TM9; 

dl~dVy [ 3 ] *  dl_dVz [2] - (Wy3*TMl+dl_dWx [2] *D3) ; 
dl_dVz [3] -  S4*TM9-C4 *TM1 3; 

dl  Wx[4]-  S5*dl_Wz [2 ] +C5*dl_Wx [3] ; 
dl_Wz [4] -  C5*dl_Wz [2] -S5*dl_Wx£3] ; 

TMO-  dl_Wx [ 3 ] *dq [4] ; 

TM1-  dl_Wz [2 ] *dq [ 4 ] ; 

TM2»  dl_dWz [ 2 ] -TMO ; 

TM3-  dl_dWx [ 3 ] +TM1 ; 
dl_dWx [  4  ]  «  S5*TM2+C5*TM3; 

TMO-  dl_dWz[3] *D4; 

TM1—  dl_Wz[3J  *D4; 

TM2-  dl_Wx[3] *D4; 

TM3-  Wz4  *TM1 ; 

TM4—  Wx4*TM2; 

TM5-  dl_Wz [3] *CM23; 

TM6-  dl_Wx [ 3 ] *CM2 4 ; 

TM7-  Wy4*TM2; 

TM8 -  dl_Wz [ 2 ] *CM2  4 ; 

TM9=  TM8-TM0; 

TM10—  TM3+TM4; 

TM11-  TM5+TM6 ; 

TM12-  TM7+TM9; 

TM1 3  -  TM1 2+dl__dVx  [31  ; 

TM14-  TM10+TM11; 

TM15—  dl_dVy [ 3 ] -TM1 4 ; 
dl_dVx [ 4 ] »  S5*TM15+C5*TM13; 

dl__dVy[4]-  - <Wy4*TMl+dl_Wz [2] *CM23+dl_dWx [ 3 ] *D4 +dl_dVz [ 3 ] ) ; 
dl~dVz [  4  ]  -  C5*TM15-S5*TM13; 

dl_dVx [ 5 ] -  S6*dl_dVy[4]+C6*dl_dVx[4] ; 
dl_dVy[5J-  Cfi*dl_dVy[4] -S6*dl_dVx[4>; 

TMO-  C6*dl_Wz [3] ; 

TM1—  S  6  *dl_Wx [ 4 ] ; 

TM2-  C6 *dl_Wx ( 4 ] ; 

TM3-  S6*dl__Wz  [  3  ]  ; 

TM4-  dl_Wz [ 3 ] *dq [ 5 ]  ; 

TM5—  dl_Wx [ 4 ] *dq[5] ; 

TM6-  dl_dWx [ 4 ] -TM4 ; 

TM7-  dl_dWz [3] +TM5; 

TM8-  TM0+TM1; 

TM9-  TM2-TM3; 

TM10-  TM8*Z6; 

TM11-  TM9*Z6; 

dl_Fx[5] -  M6*dl_dVx[5] +Wz6*TMll+dl_Wz[4] *CM22- (C6*TM7+S6*TM6) *26; 
dl__Fy (5) -  M6*dl_dVy [5] +dl_Wz [4] *CM21- (C6*TM6-S6*TM7 ) *Z6-Wz6*TM10; 
dl_Fz [ 5 ] -  M6*dl_dVz [4 ] +Wy6*TM10-Wx6*TMl 1+TM8*CM21 -TM9*CM22 ; 

dl_nx [ 5 ] -  - (26 *dl_dVy [ 5 ] ) ; 
dl_ny [5] -  Z6*dl_dVx[5] ; 
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R[5] [1]-  0.0; 

dl_f x 1 4 ] *  C6*dl  Fx[5J -S6*dl_Fy[5]  ; 
dl_fy(4]-  S6*dl~Fx [ 5] +C6*dl_Fy [5 ] ; 

dl_nx [ 4 ] -  C6*dl_nx[5]  -S6*dl_ny[5] ,• 
dl_ny [  4  ]  *  S6*dl_nx[5] +C6*dl_ny[5] ; 

R[4] [1]-  - (dl_ny [4] ) ; 

TMO-  dl_Wz[3] *Y4; 

TM1-  dl_Wx[3]*Y4; 

dl_Fx[33-  Wy4*TMl+dl_Wz[2] *CM20-dl_dWz[3)  *Y4; 
dl_Fy[3]-  - (Wz4»TM0+Wx4*TMl+dl_Wz [3] *CM1 9+dl_Wx [ 3 ] *CM20) ; 
dl_Fz[3]-  Wy4*TM0+dl_Wz[2] *CMl9+dl_dWx [3] *Y4; 

dl_fx[3]»  dl_Fx [3] +C5*dl_fx [4 ] -S5*dl_Fz [5] ; 
dl_f y [  3  ]  *  dl_Fy[3]+S5*dl“fx[4]+C5*dl_Fz[5] ; 
dl_fz[3]-  dl_Fz[3]-dl_fyl4] ; 

TMO*  AD4*dl_Wx[3]  ; 

TM1*  CD4*dl_Wz [3] ; 

dl_Nx [  3  ]  *  Wy4*TMl+dl_Wz [2] *CMl8+AD4*dl_dWx [3] ; 
dl_Ny[3]=  Wz4*TM0-Wx4*TMl+dl_Wz [3 ] * CM1 7 -dl_Wx [ 3 ] *CM18; 
dl_Nz[3j-  CD4  *dl_dWz  [  3  ]  -dl_Wz[2]  *CM17-Wy4*TM0; 

TMO*  CM1 5  *dl_f y [ 4 ]  ; 

TM1*  CM16*dl_fy  [4] ; 

TM2-  dl_nx [ 4 ] -TMO ; 

dl_nx [ 3 ] *  dl_Nx [ 3 ] +Y4  *dl_dVz [ 3 ] +C5*TM2 -S5 *TM1 ; 
dl_ny[3]-  dl_Ny [3] +SS*TM2+C5*TM1; 

dl_nz  C  3 ] -  dl_Nz [ 3 ] - ( Y4  *dl_dVx [ 3 ] +dl_ny [ 4 } +CM1 5  *dl_fx [ 4 ] -CM1 6 'dl_Fz [ 5 ] ) 

R(3] [1]-  dl_ny [  3  ] ; 

dl_fx [2] *  C4*dl_fx[3] +S4*dl_fz [3] ; 
dl_fy[2]=  S4*dl_fx[3] -C4*dl_fz[3] ; 

TMO*  CM13*dl_fy [3] ; 

TM1*  CM14*dl_fy [3] ; 

TM2*  dl_nx [ 3 ] -TMO ; 

TM3-  TMl-dl_nz [3] ; 
dl_nx[2]-  C4*TM2-S4*TM3; 
dl_ny [  2  ]  -  S4*TM2+C4*TM3; 

dl_nz [2 ] -  dl_ny [ 3 ] +CM1 3*dl_f x [ 3 ] +CM1 4 *dl_f z [3] ; 

R [2 ] [1]-  - ( dl_ny [ 2 ] ) ; 

TMO*  CMll*dl_fy [2] ; 

TM1*  CM12*dl_fy [2] ; 

TM2*  dl_nx [ 2 ] +TM0 ; 

TM3*  dl  nz ( 2 ] +TM1 ; 
dl_nx[l7-  C3*TM2-S3*TM3; 
dl_ny[l]«  S3*TM2+C3*TM3; 

dl_nz  [1]  -  CMll*dl_fx  (2 ]  -t-CMl2*dl_fy  [  3  ]  -dl_ny  [2  ]  ; 

R[l]  [1]-  dl_nz [1  ]  ; 

R C 0 ]  [1]-  S2* (dl_nx £  1 ] -ny2 ) +C2* (dl_ay { 1 ] +nx2 ) ; 


I 

void  R_matrix3 (dq, R) 
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real  dq[mm] ,  R[mm][mm]; 

{ 


real  TMO , TM1 , TM2 , TM3 , TM4 , TM5 , TM6 , TM7 , TM8 , TM9 ; 
real  TM1 0 , TM11 , TM1 2 , TM1 3 , TM1 4 , TM1 5 ; 

dl_Wx[3]-  C4*Wz3; 
dl_Wz [3] -  S4*Wz3; 

TMO-  Wz3*dq[3] ; 

dl_dWx [ 3 ] -  C4*dWz3-S4*TM0; 

dl_dWz [3] -  C4*TM0+S4*dWz3; 

TMO-  dWx3*D3; 

TM1-  Wy3*CM25; 

TM2-  TM1+TM0; 

TM3-  dVz3-TM2; 

dl_dVx [ 3 ] =  C4*TM3; 

dl_dVy [ 3 ] —  Wy3*CM26-dWz3*D3-dVx3; 

dl_dVz [3] *  S4*TM3 ; 

dl_Wx [ 4 ] -  C5*dl_Wx [3] -S5*Wx3; 
dl_Wz [4] -  - (C5*Wx3+S5*dl_Wx [3] ) ; 

TMO-  dl_Wx[3] *dq[4] ; 

TM1—  Wx3*dq[4] ; 

TM2-  dWx3+TM0; 

TM3-  dl_dWx [ 3 ] -TM1 ; 
dl_dWx [ 4 ] -  C5*TM3-S5*TM2 ; 

TMO-  dl_dWz [3] *D4; 

TM1-  dl_Wz[3]*D4; 

TM2-  dl_Wx  C 3 ] *D4; 

TM3-  Wz4*TMl; 

TM4—  Wx4*TM2 ; 

TM5-  dl_Wz [ 3 ] *CM2 3 ; 

TM6-  dl_Wx [ 3 ] *CM2 4 ; 

TM7-  Wy4*TM2; 

TM8—  Wx3*CM24; 

TM9-  TM8+TM0; 

TM10—  TM3+TM4; 

TM11—  TM5+TM6; 

TM12-  TM7-TM9; 

TM1 3 -  TM1 2  +dl_dVx [ 3 ] ; 

TM14-  TM10+TM11; 

TM1 5=  dl_dVy [ 3 ] -TM1 4  ; 
dl_dVx  C  4 ] -  S 5  * TM1 5+C5  * TM1 3 ; 

dl_dVy[4]-  - (Wy4*TMl+dl_dWx [3] *D4-Wx3*CM23+dl_dVz [3] ) 
dl_dV z [ 4 ] -  C5*TM1 5-S5  *TM1 3 ; 

dl_dVx [ 5 ] -  S6*dl_dVy [ 4 1 +C6  *dl_dVx  £  4 ] ; 
dl_dVy [5] —  C6  *dl_dVy [ 4 ] -S  6 *dl_dVx [ 4 ] ; 

TMO-  C6*dl_Wz[3] ; 

TM1-  S6*dl_Wx [4 ] ; 

TM2-  C6*dl_Wx [4 ]  ; 

TM3-  S6*dl_Wz [ 3 ]  ; 

TM4-  dl  Wz [3] *dq[5] ; 

TM5-  dl~Wx[4] *dqC5] ; 

TM6-  dl~dWx [ 4 ] -TM4 ; 

TM7-  dl'dWz ( 3 ] +TM5 ; 

TM8-  TM0+TM1; 

TM9-  TM2-TM3; 
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TM11-  TM9  *  Z  6 ; 

dl_Fx[5]«  M6*dl_dVx [5] +Wz6*TMll+dl_Wz [4 ] *CM22- (C6*TM7+S6*TM6) *Z6; 
dl~Fy [5]-  M6*dl_dVy [5] +dl_Wz [4] *CM21- (C6*TM6-S6*TM7) *Z6-Wz6*TM10; 
dl_Fz [5] »  M6*dl_dVz [4] +Wy6*TM10-Wx6*TMll+TM8*CM21-TM9*CM22; 

dl  nx [ 5 ]  -  - ( Z 6  *dl_dVy [ 5 ] ) ; 
dl_ny [  5  ]  -  Z6*dl_dVx [5] ; 

R[5] [2]-  0.0; 

dl_f x [  4  ]  -  C6*dl_Fx[5J-S6*dl_Fy:5] ; 
dl_fy [4] »  S6*dl_Fx [5] +C6*dl_Fy f5] ; 

dl_nx[4]«  C6*dl_nx[5] -S6*dl  ny(5]; 
dl_ny[4]-  S6*dl_nx [5] +C6*dl~ny [5] ; 

R[4}  [2]-  -  (dl__ny  [  4]  ) ; 

TMO-  dl_Wz [3] *Y4; 

TM1-  dl_Wx(3]*Y4; 

dl_Fx[3]»  Wy4*TMl- (Wx3*CM20+dl_dWz C3J *Y4) ; 

dl_Fy[3]-  - (Wz4*TM0+Wx4*TMl+dl_Wz [3] *CM1 9+dl_Wx [3 ] *CM20) ; 

dl_F  z  [  3  ]  —  Wy4*TM0+dl_dWx[3] *Y4-Wx3*CM19; 

dl  fx [3] *  dl_Fx[3] +C5*dl_fx[4] -S5*dl_Fz [5] ; 
dl"fy(3]~  dl_Fy  [3]  +  S5*dl_fx  [4  ]  +C5*dl_Fz  [ 5 ]  ; 
dl_f  z  [  3  ]  =  dl_Fz  [3]  -dl_fy  [ 4  ]  ; 

TMO-  AD4  *dl_Wx [ 3 ] ; 

TM1-  CD4 *dl_Wz [ 3 ] ; 

dl_Nx(3] -  Wy4*TMl+AD4*dl_dWx[3] -Wx3*CM18; 

dl_My  C  3 ] -  Wz4  *TM0-Wx4  *TMl+dl_Wz [ 3 ] *CM1 7-dl_Wx [ 3 ] *CM1 8 ; 

dl~Nz[3]-  Wx3*CM17+CD4*dl_dWz [3] -Wy4*TM0; 

TMO-  CMl5*dl_fy [4]  ; 

TM1-  CM16*dl_fy [4] ; 

TM2-  dl_nx [ 4 ] -TMO ; 

dl__ox  [  3  ]  -  dl_Nx  [ 3  ]  +Y4  *dl_dVz  [  3  ]  +C5  *TM2  -S 5  * TM1  ; 
dl~ny [3] -  dl_Ny [3] +S5*TM2+C5*TM1; 

dl_nz  [3]  -  dl_Nz [3] -  (Y4*dLl_dVx  [ 3]  +dl_ny  [ 4  ]  +CM15*dl_fx  [  4  ]  -CMl6*dl_Fz  [ 
R[31 [2] *  dl_ny[3] ; 

dl_fx[2]  -  C4*dl_fx[3] +S4*dl_fz  [3]  ;  • 

dl_fy[2]»  S4*dl_fx[3] -C4*dl_fz [3] ; 

TMO-  CM13*dl_fy [3] ; 

TM1-  CM14*dl_fy C 3 1  ; 

TM2-  dl_nx [ 3 J -TMO ; 

TM3-  TMl-dl_nz [ 3 ] ; 

C  2  ]  —  C'!*TM2-S4*TM3; 
dl_ny [2 ] -  S4*TM2+C4*TM3; 

dl_az [ 2 ] -  dl_ny C 3 ] +CM1 3 * dl_f x [ 3 ] +CM1 4  * dl_f z [ 3 ] ; 

R [2 ]  [2]-  - (dl_ny [ 2 ] ) ; 

TMO-  CMll*dl_fy [2]  ; 

TM1-  CM12*dl_fy [2]  ; 

TM2-  CM12*fy3; 

TM3-  CMll*fy3; 

TM4-  dl_nx[2] +TM0; 

TM5-  nz3+TM2; 

TM6-  dl_nz [2] +TM1; 

TM7-  nx3+TM3; 
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TM8-  TM2+TM4 ; 

TM9-  TM6-TM3; 

TM10-  TM8-TM5; 

TM11-  TM9+TM7; 

dl_nx [  1  ]  -  C3*TM10-S3*TM11; 

dl_ny [  1  ] «  S3*TM10+C3*TM11; 

dl_nz[l]-  - (CMll*fy4-CM12*fx3+dl_ny [2 ] - (CMll*dl_fx [2] +CM12*dl_fy [3] ) ) ; 
R[l]  [2]-  dl_nz [1 ]  ; 

R[0][2]«  S2*dl_nx [1 ] +C2*dl_ny [1]  ; 


} 

void  R_matrix4 {dq, R) 

real  dq[mm],  R[inm][rnmj; 

( 


real  TMO , TM1 , TM2 , TM3 , TM4 , TM5 , TM6 , TM7 , TM8 , TM9 ; 
real  TM10,TM11; 

dl_Wx[4]»  - (C5*Wz4 ) ; 
dl_Wz [4] -  S5*Wz4 ; 

TMO-  Wz4*dq[4] ; 

dl_dWx [ 4 ] -  S5*TM0-C5*dWz4; 

TMO-  dWx4 *D4 ; 

TM1-  Wy4*CM23; 

TM2-  TM1+TM0 ; 

TM3-  TM2  +dV z 4  ; 

dl_dVx [ 4 ] »  - <CS*TM3 ) ; 

dl_dVy[4]-  - (Wy4*CM24-dWz4*D4+dVx4) ; 

dl_dVz [4 ] -  S5*TM3; 

dl_dVx [  5  ]  -  S6*dl_dVy[4] +C6-dl_dVx[4] ; 
dl_dVy[5]-  C6*dl_dVy [4 ] -S6*dl_dVx [4 ] ; 

TMO-  C6*Wx4; 

TM1—  S6  *dl_Wx [ 4 ] ; 

TM2-  C6*dl_Wx[4] ; 

TM3-  S6*Wx4; 

TM4-  Wx  4  *  dq [ 5 ] ; 

TM5-  dl_Wx [ 4 ] *dq [ 5 ] ; 

TM6-  dl_dWx [ 4 ] -TM4 ; 

TM7-  dWx4+TM5; 

TM8-  TM0+TM1; 

TM9—  TM2-TM3; 

TM10-  TM8  *  Z  6 ; 

TM11-  TM9  *  Z  6 ; 

dl_Fx [5] »  M6*dl_dVx[5] +Wz6*TMll+dl_Wz [4] *CM22- (C6*TM7+S6*TM6) *26; 
dl_Fy [5] -  M6*dl_dVy [5] +dl_Wz [4] *CM21- (C6*TM6-S6*TM7) * Z6-Wz6 *TM1 0 ; 
dl_Fz [5) -  M6*dl_dVz[4] +Wy?*TM10-Wx6»TMll+TM8*CM21-TM9*CM22; 

dl_nx [ 5 ] -  - ( Z 6  *dl_dVy [  5  J )  ; 
dl_ny [5] -  Z6*dl_dVx [ 5] ; 

R[5] [3]-  0.0; 

dl_fx [4 ] -  C6*dl_Fx[5] -S6*dl_Fy [5] ; 
dl_fy (4 ] -  S6*dl_Fx [5] +C6*dl_Fy ( 5 ] ; 

HI  -.HI.  - 
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dl_ny [  4  ] »  S6*dl_ax [53 +C6*dl_ny [5]  ; 

R[4] [3]-  - (dl_ny [4] ) ; 

dl_Fx [3 ] «  - <Wy4*CM19+dWx4*Y4)  ; 
dl_Fz [3 J -  Wy4*CM20-dWz4*Y4; 

dl_f x [ 3 ] -  dl_Fx [ 3 ] +C5 *dl_fx [ 4 J -S 5 *dl  Fz[5}; 
dl_fy [3] -  S5 *dl  fx [4 J +C5*dl_Fz [S] ; 
dl~fz[3]«  dl_Fz [3] -dl_fy [4 ] ; 

TM0-  AD4 *Wz4 ; 

TM1-  CD4  *Wx4 ; 

dl_Nx [31-  Wy4*TMl-AD4*dWz4; 

dl_Ny [ 3 ] -  Wx4*CMl7+Wz4*CM18- <Wz4*TM0+Wx4*TMl) ; 
dl_Nz[3J-  Wy4*TM0+CD4*dWx4; 

TMO-  CM1 5  *dl_fy [ 4 ] ; 

TM1-  CM16*dl_fy [4 ] ; 

TM2-  dl_nx [ 4 ] -TMO ; 

dl_nx [3]-  dl_Nx [ 3 ] +Y4 *dVx4+C5 *TM2 -S  5  *TM1 ; 
dl_ny [33-  dl_Ny [3 ] +S5*TM2+C5*TM1 ; 

dl_nz  [33-  dl_Nz  [  3  ]  +Y4  *dVz 4  -  (dl_ny  [  4  ]  +CM1 5  *dl_fx  [  4  3  -CM1 6  "dl_Fz  [  5 

R[33  [31-  dl_ny [ 3 1 ; 

TMO=  dl_fx [ 3 ] +f z4 ; 

TM1-  fx4-dl_fz [3]; 

dl  f x [ 2 ] -  C4*TM0-S4*TM1; 

dl_f y [ 2 ] -  S4*TM0+C4*TM1; 

TMO-  CM13*dl_fy [3J ; 

TM1-  CM14*dl_fy  [3].; 

TM2-  CM1 4  *  fy4 ; 

TM3-  CM13*fy4; 

TM4-  dl_nx [ 3 ] -TMO ; 

TM5-  nz4-TM2; 

TM6-  dl_az [ 3 ] -TM1 ; 

TM7—  nx4-TM3; 

TM8-  TM2+TM4; 

TM9-  TM3-TM6; 

TM10—  TM8+TM5; 

TM1 1—  TM9+TM7; 

dl_nx [ 2 ) »  C4*TM10-S4 *TM1 1 ; 

dl__ny  [ 2 ]  -  S4*TM10+C4*TM11; 

dl__nz[2]  -  CM13* f Z4-CM14 * f x4+dl_ay [ 3 ] +CM13  *dl_fx [ 3 ] +CM1 4  *dl_£z [ 3 

R[2]  [3]-  - (dl_ny [2 ] )  ; 

TMO-  CM1 1 *dl_fy [2 ] ; 

TM1-  CM12*dl_fy [2]  ; 

TM2-  dl_nx [2 ] +TM0 ; 

TM3-  dl_nz [2] +TM1; 
dl_nx [11-  C3*TM2-S3*TM3; 
dl_ny [ 1 ] »  S3*TM2+C3*TM3 ; 

dl_nz [11  -  CM1 1 *dl_f x [2 ] +CM12*dl_£y ( 3 1 -dl_ny [ 2 ] ; 

R ( 1 3  [3]-  dl_nz[l] ; 

R[0] [3] -  S2*dl_nx[l] +C2*dl_ny [1] ; 


I 
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void  R_matrix5 (dq, R) 

real  dq[mm]  ,  R [nan]  [mm]/ 

{ 


real  TMO , TM1 , TM2 , TM3 , TM4 , TM5 , TM6 , TM7 , TM8 , TM9 ; 

dl_dVx[5]-  C6*dVz5; 
dl_dVy [  5  ]  -  - (S6*dVz5) ; 

TMO-  S6*Wz5; 

TM1—  C6*Wz5; 

TM2-  Wz5*dq[5] ; 

TM3-  TMO  *  2  6 ; 

TM4—  TM1 *  Z  6 ; 

dl_Fx [ 5 ] -  M6*dl_dVx[S] +Wz6*TM4- (Wx5*CM22+ (C6*TM2+S6*dWz5) *Z6) ; 
dl_Fy[5]-  M6*dl_dVy[5] -<Wz6*TM3+Wx5*CM21+ (C6*dWz5-S6*TM2) *z6) ; 
dl_F  z [ 5 ] -  Wy 6  *TM3 -Wx6  *  TM4+TMQ *  CM2 1 -TM1 *  CM2  2 -M6  *  dVx5 ; 

dl_nx [ 5 ] -  - (Z6*dl_dVy [ 5 ] ) ; 
dl_ny C 5 ] -  Z6*dl_dVx [5] ; 

R[5] [4]-  0.0; 

dl_fx [4 ] -  C6*dl_Fx [5] -S6*dl_Fy[S] ; 
dl_f y [ 4 ) -  S6*dl_Fx[S] +C6*dl_Fy[5] / 

dl_nx [ 4 ] -  C6*dl_nx [5] -S6*dl  ny[5]; 
dl_ny [ 4 ] -  S6*dl_nx[5] +C6*dl_ny [5] ; 

R[4] [4]-  - (dl_ny [ 4 ] ) ; 

TMO-  dl_fx [ 4 ] -Fz6; 

TM1-  dl_Fz[5]+fx5; 
dl_fx [ 3 ] -’  C5  *TM0-S5  *TM1 ; 
dl_fy [3] -  S5*TM0+C5*TM1; 

TMO -  CM1 5  *  dl_f y [ 4 ] ; 

TM1-  CM16*dl_fy [4]  ; 

TM2—  CM16*fy5; 

TM3-  CM15*fy5; 

TM4—  dl_nx[4] -TMO; 

TM5—  nx5-TM3; . 

TM6—  TM3+TM1; 

TM7-  TM2+TM4; 

TM8—  TM7-TM2; 

TM9-  TM6+TM5; 

dl_nx ( 3  ]  »  C5*TM8-S5*TM9; 

dl_ny [  3 ) «  S5*TM8-*-C5*TM9; 

dl_nz[3] -  CM16*fx5+CM15*Fz6- (dl_ny [4] +CM15*dl__fx  [  4  ] -CM16*dl_t z [5] ) 

R[3] [4]-  dl_ny[3] ; 

dl_fx[2] -  C4 *dl_f x [ 3 ] -S4*dl_Cy [4] ; 
dl_fy [2] -  S4 *dl_fx ( 3 ] +C4  *dl_fy [ 4 ] ; 

TMO-  CM13*dl_fy(3]  ; 

TM1-  CM1 4  *dl_f y [ 3 ] ; 

TM2-  dl_nx(3] -TMO; 

TM3-  TMl-dl_nz[3] ; 
dl_nx  [2  ]  -  c"4*TM2-S4*TM3; 
dl_ny [ 2 ] -  S4*TM2+C4*TM3; 

dl_nz [2] -  dl_ny [3] +CM13*dl_fx[ 3] -CM14*dl_fy [ 4 ] ; 

BfOirXl-  -Ml  
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TMO-  CMll*dl_fy [2] ; 

TM1-  CM12*dl_fy  [2]  ; 

TM2-  dl_nx[2] +TM0; 

TM3-  dl_nz [ 2 ] +TM1 ; 
dl_nx[l]-  C3*TM2-S3*TM3; 
dl~ny [ 1 ] -  S3*TM2+C3*TM3; 

dl~nz[l]«  CMll*dl_fx [2] +CM12*dl_fy [3] -dl_ny [2 ] ; 
R[l]  [4]-  dl_nz [1]  ; 

R[0] [4]-  S2*dl_nx[l] +C2*dl_ny [1] ; 


1 

void  R_matrix6 (dq, R) 

real  dq[mmj,  R[mm][mm]; 

{ 


real  TMO ,  TM1 ,  TM2 ,  TM3  ; 

dl_Fx [  5  ]  -  M6 *dVy 6+Wz 6  *CM2 1 -dWx  6 ' 2  6 ; 
dl_FyC5]«  - (M6*dVx6+Wz6*CM22+dWy6*26) ; 

dl_nx[5]«  26*dVx6; 
dl_ny[5]“  26*dVy6; 

R[5] [5]-  0.0; 

TMO*  dl  Fx  [5]  -Fy6; 

TM1*  dl_Fy[5]+Fx6; 
dl_fx [4 ] *  C6*TM0-S6*TMl; 
dl_fy [ 4 ] =  S6*TM0+C6*TM1; 

TMO-  dl_nx [5] -ny6; 

TM1*  dl_ny[5] +nx6; 
dl_nx [ 4 ] *  C6*TM0-S6*TM1; 
dl~ny[4]-  S6*TM0+C6*TMl ; 

R[4] [5]-  - ( dl_ny [ 4 ] ) ; 

dl_fx [3 ] -  C5*dl_fx [4 ] ; 
dl_fy [3] -  S5*dl_fx [ 4 ] ; 

TMO-  CM15*dl_fy [ 4 ]  ; 

TM1-  CMl6*dl_fy [4] ; 

TM2-  dl_nx [ 4 ] -TMO ; 

dl_nx [ 3 ] -  C5*TM2-S5*TMl; 

dl_ny [ 3 ] -  S5*TM2+C5*TM1 ; 

dl_nz [3] -  - (dl_ny [ 4 ] +CM15*dl_£x [4 ] ) ; 

R[3]  [5]-  dl_ny[3]  ; 

dl_fx [2] -  C4*dl_fx[3] -S4*dl_fy [4]  ; 
dl_fy [2] -  S4*dl_fx[3] +C4*dl_fy [4] ; 

TMO-  CM13*di_fy [3] ; 

TM1-  CM14 *dl_fy ( 3 1  ; 

TM2-  dl_nx [ 3 ] -TMO  ; 

TM3-  TM1 -dl_nz ( 3 ]  ; 
dl_nx[2]»  C4*TM2-S4*TM3; 
dl_ny(2]-  S4*TM2+C4*TM3; 

dl_nz [2] -  dl_ny[3] +CM13*dl_fx [ 3 ] -CM14*dl_fy [4 ] ; 
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R[21 [5]-  - (dl_ny [2] )  ; 

TMO-  CMll*dl_£y[2]  ; 

TMl-  CM12*dl_fy [2] ; 

TM2-  dl_nx[2] +TM0; 

TM3-  dl_nz [2] +TM1; 
dl_nx [ 1 ] -  C3*TM2-S3*TM3; 
dl_ny CH"  S3*TM2+C3*TM3; 

dl_nz[l]-  CMll*dl_fx[2]  +CM12*dl_fy  [3]  -dl_ny [2]  ; 
R[l] [5]-  dl_nz [ 1 ] ; 

R[0][5]-  S2*dl_nx[l] +C2*dl_ny [1]  ; 


} 

void  Q_matrixl (dq, Q) 

real  dq[mm]  ,  Q  [mm]  [nun]  ; 

{ 


real  TMO , TM1 , TM2 , TM3 , TM4 , TM5 , TM6 , TM7 , TM8 , TM9 ; 
real  TM1 0 , TMl 1 , TM1 2 , TM1 3 , TM1 4 , TM1 5 ; 

dl_dWx[l]»  C2*dq[ 1 ] ; 
dl_dWy[l]-  - ( S2*dq [ 1 ] ) ; 

dl_Wx [ 2 ] -  S3*C2+C3*S2; 
dl_Wz[2]-  C3*C2-S3*S2; 

TMO-  S2*dq[2] ; 

TMl-  C2*dq[2] ; 

TM2 -  dl_dWy [ 1 ] -TMO ; 

TM3-  dl_dWx [ 1 ] +TM1 ; 
dl_dWx [ 2 ] -  S3*TM2+C3*TM3; 
dl_dWz [2 ] -  C3*TM2-S3*TM3; 

TMO-  C2*A2; 

TMl-  Wx2*TM0; 

TM2-  S2*CM27; 

TM3-  Wy2*TM0 ; . 

TM4—  C2  *CM27 ; 

TM5—  TM1+TM2; 

TM6—  TM3+TM4; 

dl_dVx [2 ] -  S3*TM5-C3*TM6; 

dl_dVy [2] -  dl_dWy [ 1 ] *A2-S2*dq[l ] *A2; 

dl_dVz [2 ] -  C3*TM5+S3*TM6; 

dl_Wx [ 3 ] -  C4*dl_Wx [2] ; 
dl_Wz [3] -  S4*dl_Wx[2] ; 

TMO-  dl_Wx[2] *dq[3] ; 

dl_dWx [ 3 ] -  C4*dl_dWx[2] -S4*TM0; 

dl_dWz[3]-  C4*TM0+S4*dl_dWx[2] ; 

TMO-  dl_dWz [2 ] *D3; 

TMl-  dl_Wz[2] *03; 

TM2-  dl_Wx [2] *D3; 

TM3-  Wy3*TM2; 

TM4—  Wz3*TMl ; 

TM5-  Wx3*TM2; 

TM6-  dl  Mz[2]*CM25; 
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TM8-  TM0-TM3; 

TM9-  TM8+dl_dVx [2 ] ; 

TM10-  TM4+TM5; 

TM11-  TM6+TM7 ; 

TM12-  TM10+TM11; 

TM13-  TM12+dl  dVy [2] ; 
dl_dVx  [  3  ]  -  S  4*" TM1  3  +C  4  *  TM9  ; 

dl_dVy [  3  ]  -  dl_dVz [2 ] - (Wy3*TMl+dl_dWx [2 ] *D3) ; 
dl_dVz  C3] -  S4 ’TM9-C4  *TM1 3 ; 

dl_Wx [ 4 ] -  S5*dl_Wz [2 ] +C5*dl  Wx[3]; 
dl_Wz [4] »  C5*dl_Wz [2] -S5*dl“wx[3] ; 

TMO-  dl_Wx [ 3 ] *dq [ 4 ] ; 

TM1-  dl_Wz [2] *dq[4] ; 

TM2-  dl_dWz£2] -TMO; 

TM3-  dl_dWx [ 3 ] +TM1 ; 
dl_dWx[4]-  S5*TM2+C5*TM3; 

TMO-  dl_dWz [3] *D4; 

TM1-  dl_Wz [ 3 ] *D4 ; 

TM2-  dl_Wx [ 3 ] *D4 ; 

TM3-  Wz4*TMl; 

TM4-  Wx4*TM2; 

TM5-  dl_Wz [3] ’CM23; 

TM6-  dl_Wx [ 3 ] ’CM2  4 ; 

TM7-  Wy4  *TM2 ; 

TM8=  dl_Wz[2] *CM24; 

TM9—  TM8-TM0; 

TM10-  TM3+TM4; 

TM11-  TM5+TM6; 

TM12-  TM7+TM9; 

TM1 3 -  TM1 2 +dl_dVx [  3  ] ; 

TM14-  TM10+TM11;  . 

TM1 5-  dl_dVy [ 3 ] -TM1 4 ; 
dl_dVx [  4  ]  -  S5 ’TM15+C5  *TM13 ; 

dl_dVy [ 4 ] -  - (Wy4*TMl+dl_Wz (2] *CM23+dl_dWx [ 3 ] *D4+dl_dVz [ 3 ] ) ; 
dl_dV z [ 4 ] -  C5 *TM1 5 -S 5 *TM1 3 ; 

dl_dVx [ 5 ] -  S 6  »dl_dVy [ 4 ] +C6 *dl_dVx [ 4 i ; 
dl_dVy [ 5 ] -  C6*dl_dVy [4] -S6*dl_dVx [ 4 ] ; 

TMO-  C6*dl_Wz [3] ; 

TM1-  S6*dl_Wx [ 4 ] ; 

TM2-  C6*dl_Wx [ 4 ]  ; 

TM3-  S6*dl_Wz [ 3 ] ; 

TM4-  dl_Wz [3] *dq [ 5 ]  ; 

TM5-  dl_Wx[4] * dq [ 5 ] ; 

TM 6-  dl_dWx [ 4 ] -TM4 ; 

TM7-  dl_dWz [ 3 ] +TM5 ; 

TM8-  TM0+TM1; 

TM9-  TM2-TM3; 

TM1 0—  TM8*Z6; 

TM11-  TM9  *  Z  6 ; 

dl_Fx [ 5 ] -  M6*dl_dVx [5] +Wz6*TMll+dl_Wz [4 ] ’CM22- (C6*TM7+S6*TM6) »Z6; 
dl_Fy[5] -  M6’dl_dVy [5] +dl_Wz [4] ’CM21- (C6*TM6-S6»TM7) *Z6-Wz6 *TM1 0 ; 
dl_Fz [5] -  M6*dl_dVz [4] +Wy6*TMl0-Wx6*TMll+TM8*CM2l-TM9*CM22; 

dl_nx [5 ] -  - (Z6*dl_dVy C5] ) ; 
dl_ny [5] -  Z6’dl_dVx[5] ; 

Q C 5 1  [0]-  0.0; 

dl_fx [4 ] -  C6*dl_Fx[5] -S6»dl_Fy[5] ; 
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dl_fy[4]-  S6*dl_Fx[5]  +C6*dl_Fy [5}  ; 

dl_nx[4]  -  C6*dl_nx[51 -S6*dl_ny  [5]  ; 
dl_ny  [ 4 ]  -  S6*dl_nx [5] +C6*dl_ny [5]  ; 

QC4] [0]-  - (dl_ny [4 ] ) ; 

TMO-  dl_Wz[3] *Y4; 

TM1-  dl_Wx [3] *Y4; 

dl_Fx[3] -  Wy4*TMl+dl_Wz [2 ] *CM20-dl_dWz [3] *Y4; 

dX_Fy  C  3 ] —  - (Wz4*TM0+Wx4*TMl+dl_Wz  f  3 ] *CM19+dl  Wx [ 3 ] *CM2 0 ) ; 

dl_F~z[3)-  Wy4*TM0+dl_Wz[2] *CM19+dl_dWx[3) *Y4? 

dl_f x [  3 }  -  dl_Fx[3]+C5*dl_fx[4] -S5*dl_Fz[5] ; 
dl_fy [ 3 ] *  dl_Fy [3 ] +S5*dl_fx [4 ] +C5*dl_F2 [5] ; 
dl_f z [3] -  dl_Fz [ 3 ] -dl_fy [  4  ]  ; 

TMO-  AD4*dl_Wx[3] ; 

TM1-  CD4*dl_Wz[3] ; 

dl_Nx[31»  Wy4*TMl+dl_Wz [2] *CM1 8+AD4 *dl_dWx [ 3 ] ; 
dl_Ny [3] -  Wz4*TM0-Wx4*TMl+dl_Wz[3] 'CM1 7 -dl_Wx [ 3 ] *CM1B; 
dl_Nz [  3  ]  -  CD4*dl_dWz[3] -dl_Wz[2] *CM17-Wy4*TM0; 

TMO-  CM15*dl_fy [4]  ; 

TM1-  CMl6*dl_fy [41  ; 

TM 2=  dl_nx[4] -TMO; 

dl_nx[3]-  dl_Nx[3J+Y4*dl_dVz[3] +CS*TM2-S5*TM1; 
dl_ny [3] -  dl_Ny [ 3 ] +5S*TM2+C5*TM1 ; 

dl_nz [ 3] -  dl_Nz [ 31  - (Y4 *dl_dVx [ 3 ] *dl_r” ' 4 ] +CM1 5*dl_f x [ 4 ] -CM1 6 * 
Q [ 3 ] [01-  dl_ny[3] ; 

dl_fx[2J-  C4*dl_fx [ 3 ] +  S4  *dl_fz [3 ] ; 
dl_fy£2] -  S4*dl_fx[3] -C4*dl_fz [3] ; 

TM0=  CMl3*dl_fy [3] ; 

TM1 -  CM1 4  *  dl_f y [ 3  I ; 

TM2-  dl_nx[3J -TMO; 

TM3-  TMl-dl_nz[31 ; 
dl_ax [2 ] *  C4*TM2-S4*TM3; 
dl_ny [  2 1  -  S4 *TM2+C4 »TM3; 

dl_nz  [2]-  dl_ny  [  3  ]  +CM1 3 *dl_f  x  [  3  ]  +CM1 4  *dl..f  z  [31; 

Q [2 ] [0]-  - (dl_ny [2 ] ) ; 

TMO-  CMll*dl_fy [21 ; 

TM1-  CM12 *dl_fy [2 ] ; 

TM2-  dl_ax [2 1 +TM0 ; 

TM3-  dl_az [2] +TM1; 
dl_ax [ 1 ] -  C3*TM2-S3*TM3; 
dl_ny[l]»  S3*TM2+C3*TM3; 

dl_nz[ll-  CMll*dl_fx[2]+CMl2*dl_fy[3] -dl_ny[2i ; 

Q [ 1 :  :  0 ] -  dl_nz [ 1 ] ; 

Q  [01  [01-  RDl-t-S2*dl_nx  [  1 1  +C2*di_ny[il  ; 


void  Q_matrix2 (dq, Q) 

real  dq[mm],  Q[mm](min]; 
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real  TMO , TM1 , TM2 , TM3 , TM4 , TM5 , TM6 , TM7 , TM8 ; 

dl_dWx [ 1 ] -  C2*dq[0); 
dl_dWy [ 1 ] -  - (S2*dq[0] ) ; 

dl_dWx [  2  ]  -  S3*dl_dWy [ 1 ] +C3*dl_dWx [  1 ) ; 
dl_dWz [ 2 ] -  C3*dl_dWy r i ] -s3*dl_dWx [  1  ]  ; 

TMO-  dq[l] *A2; 

TM1-  2 . 0*TM0; 

dl_dVx [ 2 ] -  - (C3*TM1 ) ; 

dl_dVy  C  2 ] -  A2  * ( dl_dWy [ 1 ] -Wx2 ) ; 

dl_dVz [2] -  S3*TM1; 

TMO-  dl_dWx[2] -dq[3] ; 
dl_dWx [ 3 ] -  C4*TM0; 
dl_dWz [3] -  S4*TM0; 

TMO-  dl_dWz [2] *D3; 

TM1—  CM26+TM0; 

TM2-  TMl+dl_dVx [21; 

dl_dVx [  3  ]  -  S4*dl_dVy [21 +C4*TM2; 

dl_dVy [  3  ]  -  CM25-dl_dWxC2] *D3+dl_dVz [2] ; 

dl_dVz [ 3 ] -  S4*TM2-C4*dl_dVy [21 ; 

dI_Wx [ 4 ] -  - (CMO ) ; 
dl_Wz[4]«  S  5  ’  S  4  ; 

TMO-  dl_dWz [ 2 1 +CM1 l ; 

dl_dWx  [ 4  ]  *  S5*TM0*C5*dl_dWx[31 ; 

TMO-  dl_dWz [ 3 ] *D4 ; 

TM1-  CM32+TM0; 

TM2-  dl_dVx [ 3 ] -TM1 ; 

TM3-  CM3  5  +dl_dVy [  3  ]  ; 
dl_dVx [4 ] *  S5*TM3+C5*TM2 ; 

dl_dVy [ 4 ] -  - (Wy4*CM8+dl_dWx[3] *D4-dl_dVz [3] ) ; 
dl_dVz[4]»  CS*TM3-S5*TM2; 

dl_dVx  [  5  ]  -  S6*dl_dVy  [  4  ]  -*-C6*dl_dVx  [  4  ]  ; 
dl_dVy [ 5 ] -  C6*dl_dVy [ 4  I -S6*dl_dVx  [ 4  ]  ; 


TMO- 

C6  * 

CMO; 

TM1- 

S6* 

CMO  ; 

TM2- 

CMO 

*dq[5]  ; 

TM3- 

TM2 

-dl_dWz [ 3 ] 

TM4  — 

dl_ 

dWx [ 4 1 -CM7 

TM5- 

CM  6 

♦  TMO; 

TM6- 

TM1 

-CMS  ; 

TM7  — 

TM5 

*26; 

TM8- 

TM6 

*26; 

dl_Fx [ 5 ] -  M6*dl_dVx[5] +dl_Wz[41 *CM22* (C6*TM3-56*TM4 ) *26-Wz6*TM7 
di_Fy  [51  -  M6*dl_dVy  [5J  +Wz6*TM8  +  dI_Wz  [4  I  *CM21-  (S6*TM3+C6*TM4 )  *26 
dl_fZ[5]»  M  6  *  di_dv  Z [ 4 ] - Wx  6  »  TM7 - Wy  6  *  TM  8 + TM5 • CM2  2 -TM  6  *  CM2 1 ; 

di_rvx  [  5  ]  -  -  (26*dl_dVy  [51  )  ; 
dl_ny[51»  26*dl_dVx [ 5 i ; 

Q(51 [11-  0.0; 

dl_fx [4 ] »  C6  *dl_Fx [51 -S6*dl_Fy [ 5 1 ; 
dl_fy[4J-  S6*dl_Fx(51 >C6«dl_Fy [ 5 ]  ; 

dl  ax [ 4 ] -  C6*dl  ax [ 5 1  -56*dl_ny [ 5 1  ; 


I 
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dl_ny[4]«  S6*dl_nx [5 ] +C6*dl_ny [5 ] ; 

Q[4] [1]*  - (dl_ny [4] ) ; 

dl_Fx[3]-  - (Wy4*CM4+dl_dWz [3] *Y4) ; 

dl_Fy [  3  ]  -  Wx4*CM4-Wz4*CM3+S4*CM20-C4*CM19; 

dl_F  z  [  3  ]  -  Wy4*CM3+dl_dWx [3] *Y4  ; 

dl  fx[3]  -  dl_Fx [3] +C5*dl_fx [4 ] -S5*dl_Fz [ 5 ] ; 
dl_fy[3]-  dl_Fy [3]+S5*dl_fx[4] +C5*dl  Fz[5]; 
dl_fz[3] -  dl_Fz [3] -dl_fy [4] ; 

dl_Nx [ 3 ] -  Wy4*CM2+AD4 *dl_dWx [ 3  ] ; 
dl_Ny[3]-  C4*CM17+S4*CM18-(Wz4*CMl+Wx4*CM2) ; 
dl_Nz [3]-  Wy 4  *CM1+CD4 *dl_dWz [ 3 ] ; 

TMO—  CM15*dl_fy[4]  ; 

TM1—  CM16*dl_fy [4] ; 

TM2-  dl_nx [ 4 ] -TMO ; 

dl_nx [ 3 ] -  dl_Nx [ 3 ] +Y4  *dl_dVz [ 3 ] +C5  *TM2-S5  *TM1 ; 
dl_ny [3] -  dl_Ny [3] +S5*TM2+C5*TM1; 

dl_nz [ 3 ] -  dl_Nz [ 3 ] - { Y4 *dl_dVx [ 3 ] +dl_ny [ 4 ] +CM1 5 *dl_f x [ 4 ] -CM1 6 *dl 

Q [ 3 ] [li-  dl_ny [ 3 1 ; 

dl_fx [2] -  C4*dl_fx[3] +S4*dl_f z [3] ; 
dl_fy  [2]  »  S4*dLl_fx[3]  -C4*dl_fz[3]  ; 

TMO-  CM13*dl  f y [31; 

TM1-  CM1 4 »dl_f y [ 3 ]  ; 

TM2-  dl_nx [ 3 ] -TMO ; 

TM3-  TMl-dl_nz [3]  ; 
dl_nx  [  2  ]  »  C4*TM2-S4*TM3; 
dl_ny [ 2 ] -  S4*TM2+C4*TM3; 

dl_nz [2 ] -  dl_ny [3] +CM13*dl_fx[3] +CM14*dl_fz [3] ; 

QC21 [1]-  - (dl_ny [2] ) ; 

TMO-  CMll*dl_fy [2]  ; 

TM1-  CM12*dl_fy[2] ; 

TM2-  dl_nx [ 2 ] +TMO ; 

TM3-  dl_nz [2] +TM1; 
dl__nx  [  1  ]  »  C3*TM2-S3*TM3; 
dl_ay [ 1 1 »  S3*TM2+C3*TM3; 

dl_nz  [1  ]  -  CM1 1  *dl_£x  [ 2  ]  1-CM12 *dl_fy  [  3  ]  -dl_ny[2]  ; 

Q [11 [11-  RD2+dl_nz[l] ; 

Q  C 0 1  [ 1 ] -  S2 *dl_nx [ 1 ] +C2  *dl_ay [11; 


void  Q_macrix3 (dq,  Q) 

real  dq[mm],  Q [mm]  [mm)  ; 

i 


real  TMO , TM1 , TM2 , TM3 , TM4 , TM5 , TM6 , TM7 , TM8 ; 


dl_dWx [2 ] -  C3*Wy2-S3*Wx2; 
dl_dWz [2] -  - (C3*Wx2+S3*Wy2) ; 


TMO 

rtl 


dl_dWx [ 2 ] -dq ( 3 ]  ; 
it  f  7  i  ■  n 


Fr  [51  ) 


•IT 
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dl_dWz [3]  -  S4*TM0; 

TMO-  dl_dWz [2] *D3; 

TM1-  CM2  6+ TMO; 

dl_dVx [  3  ] «  C4*TM1; 

dl_dVy [ 3 ] -  CM25-dl_dWx[2] *D3; 

dl_dVz  [3]  -  S4*TM1; 

dl_Wx [ 4 ] -  - {CMO ) ; 
dl_Wz [ 4 ] -  S  5  *  S  4 ; 

TMO-  dl_dW  z  [  2  ] +CM1 0 ; 
dl_dWx[41«  S5*TM0+C5*dl_dWx [ 3 ] ; 

TMO-  dl_dWz [3] *D4 ? 

TM1-  CM3 2 + TMO; 

TM2-  dl_dVx[3J -TMl; 

TM3-  CM35+dl_dVy[3] ; 
dl_dVx [ 4 ] -  S5*TM3+C5*TM2; 

dl_dVy [ 4 ] -  - (Wy4*CM8+dl_dWx [3 ] *D4+dl_dVz [3 ] ) ; 
dl_dVz [4  3-  C5*TM3-S5*TM2; 

dl_dVx [53-  S 6 *dl_dVy [ 4 ] +C6  *dl_dVx [  4  ]  ; 
dl_dVy [53=  C6  *dl_dVy [ 4 ] -S  6  *dl_dVx [43; 

TMO—  C6*CM0 ; 

TM1—  S6*CM0; 

TM2-  CM0*dq[51; 

TM3-  TM2 -dl_dWz [33; 

TM4-  dl_dWx[4] -CM7; 

TM5-  CM6+TM0; 

TM6»  TMl -CMS; 

TM7-  TM5  *  Z 6 ; 

TM8-  TM6*Z6; 

dl_Fx [51-  M6  * dl_dVx [ 5 1 +dl_Wz [ 4 ] * CM2 2  +  ( C 6  * TM3  - S  6  * TM4  )  *  Z  6 - Wz  6  *  TM7  ; 
dl_Fy [5J-  M6  *dl_dVy [ 5  3  +Wz 6  *TM8 +dl_Wz [ 4 ] *CM2 1 - ( S  6  *TM3  +C6  * TM4 ) *  Z  6 ; 
dl_F  z [ 5 ] =  M6 *  dl_dV  z  [  4  ]  +  Wx  6  *  TM7  -  Wy  6  *  TM  8 +TM5 ’ CM2  2  - TM  6  * CM2 1 ; 

dl  nx [5] -  - (Z6*dl_dVy [5] )  ; 
dl_ny [ 5 ] -  Z6*dl_dVx [ 5 3 ; 

Q [53  C21-  0.0; 

dl_fx [ 4 ] -  C6*dl_Fx[5] -S6*dl_Fy [5] ; 
dl_fy [ 4 ] =  S6*dl_/x[5]+C6»dl_Fy[5] ; 

dl_nx [ 4 ] =  C6*dl_nx[5] -S6*dl_ay[5] ; 
dl_ny [ 4 ] =  S6*dl_nx ( 5 ] +C6*dl_ay [53; 

Q[43  [23-  - (dl_ny [ 4 ] ) ; 

dl_Fx [ 3 ] »  - (Wy4*CM4+dl_dWz [3] *Y4) ; 

dl_Fy [3] -  Wx4*CM4-Wz4*CM3+S4*CM20-C4*CM19; 

dl_Fz [ 3 ] -  Wy4*CM3+dl_dWx[3] *Y4; 

dl_fx[3] -  dl_Fx [ 3 ] +C5 *dl_fx [ 4 ] -S5*dl_Fz[5] ; 
dl  Ey  [  3  1  -  dl_Fy[31-*-S5*dl_fx[4)+C5»dl_Fz[53  ; 
dl~fz[3]-  dl_Fz [ 3 1 -dl_fy [ 4 ] ; 

dl_Nx ( 3 ] -  Wy4 *CM2+AD4 *dl_dWx [ 3 1 ; 

dl_My[31-  C4*CM17+S4*CM18- (Wz  4  *CMH-Wx4  *CM2  )  ; 

dl_Nz [ 3 ] -  Wy 4  *CM1 +CD4  *dl_dWz [ 3 ) ; 


TMO-  CM15*dl_£y (4 ] ; 
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TM1-  CM16*dl_fy  [4]  ; 

TM2-  dl_nx [ 4 ] -TK0 ; 

dl_nx  C  3  ]  —  dl_Nx  C  - )  +Y4  * dl_dVz  [  3  ]  -*C  5  * TM2  - S 5  * TM1  ; 
dl_ny C 3 ] »  dl_Ny [3] +SS*TM2+C5*TM1; 

dl_nz [ 3 ] -  dl_Nz [ 3 ] - { ¥4  *dl_dVx [ 3 ] +dl_ny  C  4 ] +CM1 5  *dl_f x [ 4 ] -CM1 6  * dl_F  z [ 5 ] 
Q [ 3 ] [2]-  dl_ny[3] ; 


dl_fx[2]-  C4*dl  fx [3] +S4*dl_f z [3 ] ; 
dl_f y [ 2 ] -  S4*dl_fx[3] -C4*dl_fz [3] ; 

TMO-  CM1 3  *dl_fy [ 3 ] ; 

TM1-  CM14*dl_fyC3] ; 

TM2-  dl_nx [ 3 ] -TMO ; 

TM3-  TMl-dl_nz[3] ; 
dl_nx[2]-  C4*TM2-S4*TM3; 
dl_ny[2]-  S4*TM2+C4*TM3; 

dl_nz[2]-  dl_ny [3] +CM13*dl_fx [ 3 ] +CM14*dl_f z [3 ] ; 

Q[2] [2]-  RD3-dl_ny[2] ; 

TMO-  CMll*dl  f y [ 2 ] ; 

TM1 =  CM1 2  *  dl_f y [2] ; 

TM2-  dl_nx[2l+TM0; 

TM3-  dl_nz [ 2 ] +TM1 ; 
dl_nx [ 1 ] -  C3*TM2-S3*TM3; 
dl_ny [  1  ]  -  S3*TM2+C3*TM3; 

dl~nz [ 1 ] -  CMll*dl_fx [2 ] +CM12*dl_fy [ 3 ] -dl_ny [2] ; 
Q [ 1 ] [ 2 ] -  dl_n  z ( 1 ] ;  . 

Q [ 0 ] [2]-  S2*dl_nx [ 1 ] +C2*dl_ny [ 1 ] ; 


void  Q_matrix4 (dq, Q) 

real  dq[mm],  Q[mm][mm]; 

{ 

real  TMO , TM1 , TM2 , TM3 , TM4 , TM5 ; 

dl_dWx[3]«  C4*Wy3-S4*Wx3; 
dl_dWz[31-  C4*Wx3+S4*Wy3; 

TMO-  dl  dWx [ 3 ] +dq ( 4 ] ; 
dl_dWx [7] -  C5*TM0; 

TMO-  dl_dWz [3] *D4; 

TM1-  CM2 4- TMO; 

dl_dVx [ 4 ] -  C5*TM1 ; 

dl_dVyC41-  - (CM2 3+dl  dWx[3I*D4); 

dl_dV z [ 4 ] -  -<S5»TM1)7 

dl_dVx [ 5 ] -  S6*dl_dVy [4 ] +C6 »dl_dVx { 4 ] ; 
dl_dVy [ 5 ] -  C6*dl_dVy [ 4 ] -S6*dl_dVx [4] ; 

TMO-  S 6  *  S  5 ; 

TM1-  C6*S5; 

TM2-  S5*dq[5] ; 

TM3-  TM0*Z6; 

TM4-  TM1 *  Z  6 ; 

TM5-  dl_dWz [ 3 ] +TM2 ; 

dl  FxfSl-  ftVx  f  5  1  +Wz6»TM4+r^  »TM7  ?  -  f  rg  1  muv  f  a  w 
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dl_Fy [5]-  M6*dl_dVy [5] +C5*CM21- (C6*dl_dWx [4 ] -S6*TM5) *Z6-Wz6*TM3; 
dl_Fz [5] -  M6*dl_dVz[4]+Wy6*TM3-Wx6*TM4+TM0*CM21-TMl*CM22; 

dl_nx[51-  - ( Z  6  *dl_dVy [  5  ]  )  ; 
dl_ny  [  5  J  -  Z6*dLl_dVx[5]  ; 

Q[5] [3]-  0.0; 

dl_f x [  4  ]  -  C6*dl_Fx[5] -S6*dl_Fy [51  ; 
dl_fy[4]»  S6*dl_Fx[5] +C6*dl_Fy[5] ; 

dl_nx [ 4 ] —  C6 *dl_nx [5] -S6*dl_ny [5] ; 
dl_ny[4]-  S6*dl_nx[5] +C6*dl_ny [51 ; 

Q [4] [31-  ~(dl_ny[4] ) ; 

dl_Fx [ 3 ] -  CM20-dl_dWz [3] *Y4 ; 
dl_Fz [3] -  CM1 9+dl_dWx [ 3 ] * Y4 ; 

dl_fx [ 3] -  dl_Fx [3] +C5*dl_fx [4 ] -S5*dl_Fz [5] ; 
dl_fy [3] -  S5*dl_fx [ 4 ] +C5*dl_Fz [ 5 ] ; 
dl_f z [3] -  dl_Fz [ 3 ] -dl_fy [  4  J  ; 

dl_Nx [ 3 ] -  CM1 8+ AD 4  *dl_dWx [ 3 ] ; 
dl_Nz[3]»  CD4*dl_dWz [3] -CM17; 

TM0-  CM15*dl_fy [4] ; 

TM1-  CM16*dl_fy [4 ] ; 

TM2-  dl_nx [ 4 ] -TMO ; 

dl_nx [ 3 ] -  dl_Nx[3] +CS*TM2-SS*TM1; 

dl_ny [ 3 ] -  S5*TM2+C5*TM1; 

dl_nz  [3]  dl_Nz[3]  -'(dl_ny  [4] +CM15*dl_fx[4]  -CM16*dl_Fz  [5]  )  ; 

Q[3] [31-  RD4+dl_ny[3] ; 

dl_fx [2] -  C4*dl_fx[3J+S4*dl_fz[3] ; 
dl_fy [2 ] -  S4*cu_fx  [3  ] -C4*dl_f z [ 3 ) ; 

TMO—  CM13*dl_fy [3] ; 

TM1-  CM14*dl_fy [3] ; 

TM2-  dl_nx [ 3 ] -TMO ; 

TM3-  TMl-dl_nz[3] ; 
dl_nx[2]-  C4*TM2-S4*TM3; 
dl_ay [2 ] -  S4 *TM2+C4 *TM3 ; 

dl_nz [21  -  dl_ny [ 3 ] +CM1 3 * dl_f x [ 3 ] +CM1 4 * di_f z [ 3 ] ; 

Q [21 [31-  - ( dl_ny [ 2 ] ) ; 

TMO-  CMll*dl_fy [2]  ; 

TM1-  CM12*dl_fy [2 1 ; 

TM2-  dl_nx [2 1 +TM0 ; 

TM3-  dl_nz[2] +TM1; 
dl_nx [ 1 ] -  C3*TM2-S3*TM3 ; 
dl_ny [1 ] -  S3*TM2+C3*TM3; 

dl_nz[ll-  CMll*dl_fx [ 2 1 +CM12*dl_fy [ 3 1 -dl_ny [ 2 ] ; 

Q [ 1 1 [31-  dl_nzfl] ; 

Q [ 0 1  [3]-  S2*dl_nx [ 1 1 +C2*dl_ny  f  l  ]  ; 


void  Q_matrix5 (dq, r' 
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real  dq[mm],  2 [nan]  [ram]; 

1 


real  TMO , TM1 , TM2 , TM3 ; 

dl_dWx[4]-  C5*Wy4-S5*Wx4; 

TMO-  S  6  *  Z  6 ; 

TMl—  C6*Z6; 

TM2-  dl_dWx [ 4 ] -dq [  5  ]  ; 

dl_Fx[Sf-  - (Wz6*TM0+S6*TM2*Z6) ; 

dl_Fy  [5]-  -  (Wz6*TMl+C6*TM2*Z6)  ; 

dl_Fz [51-  Wx6*TM0+Wy6*TMl+S6*CM22+C6»CM21; 

Q[5]  [4]-  0.0; 

dl_fx [4 ] -  C6*dl_Fx [ 5 ] -S6*dl_Fy [ 5 ]  ; 
dl~f y [ 4 ] »  S6*dl_Fx [5] +C6*dl_Fy [5] ; 

Q  [4 ]  [4]-  RD5; 

dl_fx [  3  ]  -  C5*dl_fx [ 4 ] -S5  *dl_F z [ 5 ] ; 
dl_fy [3] -  S5*dl_fx [ 4 ] +C5*dl_F z [ 5 ]  ; 

TMO -  CM1 6  *  dl_fy [ 4 ] ; 

TMl —  CM15*dl_fy [4  ]  ; 

dl_nx [ 3 ] »  -  (C5*TM1+S5*TM0) ; 

dl__ny  [  3 ]  -  C5*TM0-S5*TM1; 

dl_nz  C3] -  CM16*dl_Fz[5] -CM15*dl_fx[4] ; 

Q [ 3 ] [  4  ]  -  dl_ny [ 3 ] ; 

dl_fx [2 ] -  C4*dl_fx[3] -S4*dl_fy [4] ; 
dl_fy [2] -  S4*dl_fx [ 3 ] +C4*dl_fy [ 4 ] ; 

TMO-  CMl3*dl_fy [3] ; 

TMl-  CM14*dl_fy [3] ; 

TM2-  dl_nx[3] -TMO; 

TM3-  TMl-dl_nz [3] ; 
dl_nx [ 2 ] -  C4*TM2-S4*TM3; 
dl_ny [ 2 ] -  S4*TM2+C4*TM3; 

dl_nz [2 ] -  dl_ny [ 3 ] +CM1 3 *dl_fx [ 3 ] -CM14 *dl_fy [4 ] ; 

Q [ 2 ] [4]-  ~(Hi_ny[2] ) ; 

TMO-  CM11 *dl_fy (2 ] ; 

TMl -  CM1 2  * dl_f y [ 2 ] ; 

TM2-  dl_nx[2] +TM0; 

TM3-  dl_nz [2] +TM1; 
dl_nx [ 1 ] -  C3*TM2-S3*TM3; 
dl~ny [ 1 ] »  S3*TM2+C3*TM3; 

dl_nz [ 1 ] -  CMll*dl_fx[2] +CM12*dl_fy [3] -dl_ny[2] ; 
Q C 1 J  [4]-  dl_nz [1J; 

Q ( 0 ]  [4]-  S2*dl_nx [ 1 ] +C2*dl_ny [ 1 ]  ; 


I 

void  Q_matrix6 (dq, Q) 

real  dq(nan],  Q[mm][mm]; 
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real  TMO , TM1 , TM2 , TM3 ; 

dl_Fx  C 5 1  *  CM22- (C6*Wx5+S6*Wy5 ) *Z6; 
dl_Fy[5]-  CM21- (C6*Wy5-S6*Wx5) *Z6; 

Q [ 5 ]  [5]-  RD6; 

dl_f  x [  4  ]  -  C6*dl_Fx [5] -S6*dl_Fy [5] ; 
dl_fy [4] *  S6*dl_Fx[5]+C6*dl_Fy[5] ; 

Q[4] [5]-  0.0; 

dl_fx [3  ]  -  C5*dl_fx[4]; 
dl_fy[3]-  S5*dl_fx[4] ; 

TMO-  CMl6*dl_fy [4] ; 

TM1-  CM15*dl_fy [4] ; 
dl_nx [ 3 ] -  - (CS*TM1+S5*TM0) ; 
dl_ny [ 3 ] -  C5*TM0-S5*TM1; 
ll_nz  [3]  -  - (CM15*dl_fx [4 ] ) ; 

Q [3] [5]-  dl_ny[3]  ; 

dl_fx [2] *  C4*dl_fx [3] -S4  *dl_fy [  4  ] ; 
dl_fyC2]-  S4*dl_fx [3] +C4*dl_fy [4 ] ; 

TMO=  CM13*dl_fy [3] ; 

TM1-  CM1 4  *dl_f y [ 3 ] ; 

TM2=  dl_nx [ 3 ] -TMO ; 

TM3-  TMl-dl_nz [3] ; 
dl_nx [  2  ]  -  C4*TM2-S4*TM3; 
dl_ny [ 2 ] -  S4*TM2+C4*TM3; 

dl_nz [2] -  dl_ay [3 ] +CM13*dl_fx [ 3 ] -CM14*dl_fy [4] ; 

Q [2 ] [5]-  - (dl_ny [2] ) ; 

TMO-  CM11 *dl_fy [2 ]  ; 

TM1-  CM12*dl_fy [2] ; 

TM2-  dl_nx [2] +TMO ; 

TM3-  dl_nz [2 ] +TM1 ; 
dl_nx [  1  ] »  C3*TM2-S3*TM3; 
dl_ny [ 1 ] -  S3*TM2+C3*TM3; 

dl_nz  [  1  ]  »  CMll.*dl_fx  [2  ]  +CM12*dl_fy  [ 3  i  -dl_ny  [2  ]  ; 
Q [11  [5]  -  dl_nz  [  1  ]  ; 

Q CO  1  [5]-  S2*dl_nx[ll+C2*dl_ny  [1] ; 


1 

void  P_matrixl(P) 
real  P [mm] [mm] ; 

( 


real  TMO , TM1 , TM2 , TM3 ; 

dl_dWx [2] -  S3*C2+C3*S2 ; 
dl_dWz [2 ] -  C3*C2-S3*S2; 

dl_dVy [ 2 ] -  C2*A2; 

dl_dWx [ 3 ] -  C4 *di_dWx [ 2 ] ; 
dl_dWz [3 ] -  S4*dl_dWx [2 ] ; 
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TM0-  dl_dWz[2] *D3; 
dl_dVx [ 3 ] -  S4*dl_dVy [2] +C4*TM0; 
dl_dVy [  3  ]  -  - (dl_dWx[2] *D3)  ; 
dl_dVz [3] -  S4*TM0-C4*dl_dVy [2] ; 

dl_dWx [ 4 ] -  S5*dl_dWz [2] +C5*dl_dWx [ 3 ] ; 

TMO-  dl_dWz [3] *D4; 

TM1 -  dl_dVx [ 3 ] -TMO ; 
dl_dVx [  4  ] «  S5*dl_dVy [3] +C5*TM1; 
dl_dVy £ 4 ] -  - (dl_dWx £ 3 ] *D4 +dl_dVz [  3  ] ) ; 
dl_dVz[4]»  C5*di_dVy [3] -S5*TM1; 

dl_dVx [ 5 ] -  S6*dl_dVy ( 4 ] +C6*dl_dVx [ 4 ] ; 
dl_dVy[5]-  C6*dl_dVy [4 ] -S6*dl_dVx [4 ] ; 

dl_Fx [  5  ]  -  M6*dl_dVx C5] - (C6*dl_dWz  [3 ] +S6*di  dWx[4])*Z6; 
dl_Fy [  5  ] «  M6*dl_dVy [5] - (C6*dl_dWx [4] -S6*dl~dWz [3] ) *Z6; 
dl_Fz [5] -  M6  *dl_dVz [ 4 ]  ; 

dl_nx[5]-  - ( Z 6 *dl_dVy [ 5 ] ) ; 
dl_ny [  5  ]  -  Z6*dl_dVx [5] ; 

P [5] £0]-  0.0; 

dl_f x [  4  ]  -  C6*dl_Fx [ 5 ] -S6*dl_Fy[5] ; 
dl_£y[4]-  S6*dLl_Fx  [5]  +C6*dl_Fy  [5]  ; 

dl_nx [4 ] =  C6*dl_nx [5] -S6*dl_ny [5]  ; 
dl_ny [ 4 ] *  S6*dl_nx [ 5 ] +C6*dl_ny [ 5 ] ; 

P[4]  [0]-  - (dl_ny [4 ] )  ; 

dl_Fx[3]-  - (dl_dWz [3] *Y4) ; 
dl_Fz [ 3 ] -  dl_dWx [ 3 ] *Y4 ; 

dl_fx[3]-  dl_Fx [3] +C5*dl_fx [ 4 ] -S5*dl  Fz[5]; 
dl_fy[3]-  S5*dl_fx [4 ] +C5 *dl_Fz [ 5 } ; 
dl_fz[3]«  dl_Fz[3] -dl_fy[4] ; 

dl_Nx[3] -  AD4*dl_dWx[3] ; 
dl_Nz[3]-  CD4*dl_dWz [3] ; 

TMO-  CMl5*dl_fy [4]  ; 

TM1 -  CM1 6  *dl_fy C  4 ] ; 

TM2-  dl_nx [ 4 ] -TMO ; 

dl_nx [31-  dl_Nx [ 3 ] + Y  4  * dl_dV  z [ 3 ] +C  5  *  TM2 - S  5  *  TM1 ; 
dl_ny[3]-  S5*TM2+C5*TM1 ; 

dl_nz [  3  ] »  dl_Nz[3] - <Y4*dl_dVx [ 3 ] +dl_ny [ 4 ] +CM1 5*dl_fx ( 4 ] -CM16*dl_Fz  [5] ) 

P [ 3 ]  [ 0 ] -  dl_ay [3]  ; 

dl_fx[2I-  C4*dl_fx[3] +S4*dl_fz [3] ; 
dl_fy [2] «  S 4 * dl_f x [ 3 ] -C  4  *  dl_  £  z ( 3 ] ; 

TMO-  CM13*dl_fy £3]  ; 

TM1-  CM14*dl_fy [3]  ; 

TM2-  dl_nx [ 3 ] -TMO ; 

TM3-  TMl-dl  nz [3] ; 
dl_nx [2 1  -  C4»TM2-S4*TM3; 
dl_ny £21-  S4*TM2+C4*TM3; 

dl_nz  £2 ] -  dl_ny [ 3 ] +CMl3*dl_fx [ 3 1 +CM14  *dl_f z [ 3 ] ; 
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PC2] [0]-  - (dl_ny [2] ) ; 

TM0-  CMll*dl_fy  [2] ; 

TM1-  CM12*dl_fy  [2]  ; 

TM2-  dl_nx[2] +TMO; 

TM3-  dl_nz [2 ] +TM1 ; 
dl_nx [  1  ]  -  C3*TM2-S3*TM3; 
dl_ny [ 1 ] *  S3*TM2+C3*TM3; 

dl_nz [1 ] -  CM1 1 *dl_f x [ 2 ] +CM1 2 * dl_f y [ 3 ] -dl_ny [2 ] * 
PCI]  [0]-  <±l_nz  [  1  ]  ; 

P  C 0 ]  CO]-  IA1+S2*dl_nx[l]+C2*dl_ny[l] ; 


void  P_matrix2(P) 
real  P [mm] [mm] ; 

( 

real  TMO , TM1 , TM2 , TM3 ; 

dl_dVx [ 3 ] -  C4*CM11; 
dl_dVz [3] -  S4*CM11; 

dl_dWx [ 4 ] =  - ( CMO ) ; 

TMO-  dl_dVx [ 3 ] -CM8 ; 
dl_dVx [ 4 ] -  S5*CM12+C5*TM0 ; 
dl_dvy [ 4 ] -  CM9-dl_dVz[3] ; 
dl_dVz [4 ] -  C5*CM12-S5*TMQ ; 

dl_dVx [ 5 ] -  S6*dl_dVy [4] +C6*dl_dVx[4 ] ; 
dl_dVy[5]-  C6*dl_dVy [4] -S6*dl_dVx [ 4 ] ; 

dl_Fx  [ 5  ]  -  M6 *dl_dVx  [ 5  ]  +  ( S 6 *CM0 -C.M5 )  *Z6; 
dl_Fy [5] =  M6*dl_dVy [5] + (CM6+C6*CM0) *Z6; 
dl_F z [ 5 ] =  M6*dl_dVz [ 4 ]  ; 

dl_nx [ 5 ] -  - (Z6*dl_dVy [5] ) ; 
dl_ny[5]»  Z6*dl_dVx [5] ; 

P [ 5 ] [1 ] -  0.0;- 

dl_fx[4]  -  C6*dl_Fx[5]  -S6*dl_Fy  [5]  ; 
dl_f y [ 4 ] -  S6*dl_Fx [ 5 ] +C6*dl_Fy [ 5 ] ; 

dl_nx [ 4 ] -  C6*dl_nx [5] -S6*dl_ny(5] ; 
dl_ny [ 4 ] -  S6*dl_nx[5] +C6*dl_ny [5]  ; 

P [4 ] [1]-  - (dl_ny [ 4 ] ) ; 

dl_Fx [ 3 ] -  -(CM3); 

<11_F  z  [  3  ]  -  -  ( CM4 )  ; 

dl_fx[3] -  C5 *dl_f x [ 4 ] -S5*dl_F z [ 5 ] -CM3 ; 
dl_fy [3] -  S5*dl_fx [4 ] +C5*dl_Fz [ 5 ] ; 
dl_f z [31-  - ( CM4  +dl_f y [ 4 ] ) ; 

dl_Nx [ 3 ] -  - (CMl ) ; 

TMO-  CM15*dl_fy[4) ; 

TM1-  CMlt>*dl_fy  [4]  ; 
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TM2-  dl_nx [ 4 ] -TMO ; 

dl_nx [ 3 ] -  Y4*dl_dVz[3]+C5*TM2-S5*TMl-CMl; 
dl_ny [  3  ]  -  S5*TM2+C5*TM1; 

dl_nz [3] -  CM2 -  ( Y  4  * dl_dVx [ 3 ] +dl_ny [ 4 ] +CM1 5 * dl_f x  [  4  ] 

P[3] [11-  dl_ny [ 3 ] ; 

dl_fx [2] -  C4*dl_fx[3] +S4*dl_fz [3] ; 

TMO-  CM13*dl_fy [3] ; 

TM1-  CM14*dl~fy[3] ; 

TM2-  dl_nx[3] -TMO; 

TM3-  TMl-dl_nz  [3] ; 
dl_ny [21-  S4*TM2+C4*TM3; 

P[2] [I]-  - (dl_ny [2] ) ; 

P[l] Cll-  IA2+CMll*dl_fx[2]+CM12*dl_fy[3] -dl,  ny(2] ; 
p  CO ]  ci] -P Cll  [0]  ; 


} 

void  P_matrix3(P) 
real  P [mm] [mm] ; 

( 


real  TMO , TM1 , TM2 ; 

dl_dWx  1 4 ] -  - ( CMO ) ; 

dl_dVx [ 4 ] -  - (C5*CM8 ) ; 
dl_dVz [4] ■  S5*CM8; 

dl_dVx [ 5 ] -  S6*CM9+C6*dl_dVx[4] ; 
dl_dVy [ 5 ] -  C6 *CM9-S  6  *dl_dVx [  4  ]  ; 

dl_Fx (5] -  M6*dl_dVx[5]  +  (S6*CM0-CM5) *26; 
dl_Fy [5] -  M6*dl_dVy[5] + (CM6+C6*CM0) *Z6; 
dl_F z [ 5 ] -  M6*dl_dVz[4] ; 

dl_nx [ 5 ] *  -(26 *dl_dVy [ 5 ] ) ; 
dl_ny [ 5 ] =  Z6*dl_dVx [5] ; 

PCS] C2] -  0.0; 

dl_fx [ 4 ] -  C6*dl_Fx[5] -S6*dl_Fy [5]  ; 
dl_fy[4]-  S6*dl_FxC5] +C6*dl_Fy[5] ; 

dl_nx [ 4 ] -  C6*dl_nxC5] -S6*dl_ny (51 ; 
dl_ny [4] -  S6*dl_nx[5] +C6 *dl_ny [5 ] ; 

P(4] [2]-  - (dl_ny (4 ] ) ; 

dl_fy ( 3 ] -  S5*dl_fx ( 4 ] +CS*dl_Fz ( 5 ]  ; 

dl_Nx ( 3 ] -  - (CM1 ) ; 

TMO-  CM15*dl  f y ( 4 ] ; 

TMl—  CM16*dl  fy [4]  ; 

TM2-  dl_ax  ( 4  ~-TMO ; 

dl_nx ( 3 ] -  C5  *TM2-S5*TM1 -CM1 ; 

dl  nv f  3 1 »  <4S«TM9*rs«TMi  ; 


-CM1 6 * dl  Fz [ 5 ]  ) 


PUMA260 .  C 


Wad  Mar  8  17:51:04  1989 


28 


dl_nz [3 ] =  CM2- (dl_ny [4 ] +CM15*dl_fx [4  J -CM16*dl_Fz [ 5 ] )  ; 

P [3]  [2]*  dl_ny [ 3 ]  ; 

P [2]  [2 ] *  I A3- (S4 * (dl_nx[3] -CM13*dl_fy [2] ) +C4* (CM14*dl_fy [ 3 ] -dl_nz [ 3 ] ) ) 
PCI]  [2] *P [2]  CU  ; 

P[01  [2]*P[2]  [0]  ;  _ c  _  , _ 

} 

void  P_matrix4 (P) 
real  P [mm] [mm] ; 

( 

dl_Fx [ 5 ] *  -<S6*S5*Z6); 
dl_Fy [5] *-(C6*S5*Z6)  ; 

PC 5]  [3]*  0.0; 

dl_fy [ 4 ] -  S6*dl_Fx [ 5] +C6*dl_Fy [5]  ; 

P [ 4 ] [ 3 ] *  0.0; 

P [3]  [3] ”  IA4+C5*CM1 6*dl_fv [ 4 ] -S5*CM15*dl_fy f 4 ] ; 

?  [2]  [3]  *P  [3]  [2]  ; 

P  CU  [3]  -P  [3]  [1]  ; 

P[0]  [3] -? [3]  [0]  ; 

} 

void  P_matrix5(P) 
real  P [mm] [mm] ; 

{ 

PCS] [4]-  0.0; 

? [ 4 ] [ 4 ] -  IA5 ; 

P [3]  [ 4 ] *P [4 ]  [3]; 

P  [2]  [ 4 ] *P [ 4 ]  [2]  ; 

PCI]  [ 4 ] *P  [  4 ]  [I]; 

P[0]  [4] -P[4]  [0]  ; 


t 

void  P_matrix6(P) 
real  P [mm] [mm] ; 

( 


PCS] [5]-  IAS; 
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P[4] 

[5] 

-P[5] 

[4]  ; 

P[3] 

[5] 

-PCS] 

C3]; 

P  [2] 

[5] 

-P  [5  J 

C2]; 

PCI] 

[5] 

-P  [5] 

[1] ; 

P[0] 

[5] 

-PCS] 

CO]  ; 

void  P_matrix(P) 

real  P [mm] [mm] ; 

{ 

P__matrixl  (P)  ; 
P_jnatrix2 (P)  ; 
P_matrix3 (P)  ; 
P_matrix4 (P)  ; 
P_matrix5 (P)  ; 

P  matrix6 (P)  ; 


) 

void  Q_matrix (dq, Q) 

real  dq[mm] ,  Q[mm] [mm] ; 

{ 


Q_matrixl (dq,Q)  ; 
Q_matrix2 (dq,  Q) ; 
Q_matrix3 (dq,  Q)  ; 
Q_matrix4 (dq,Q)  ; 
Q_matrix5 (dq,  Q)  ; 
Q_matrix6 (dq,  Q)  ; 


) 

void  R_matrix (dq, R) 

real  dq[mm] ,  R[mm](mm]; 

( 

R_matrixl (dq,  R)  ; 
R_matrix2 (dq,  R)  ; 
R_matrix3 (dq,  R)  ; 
R_matrix4 (dq, R) ; 
R_matrix5 (dq,  R)  ; 
R_matrix6 (dq,  R) ; 


/»****«■»*******,».*„  ***«*****„„„,*„»»„*„*„ 

Additions  :  473 

Substractions  :  451 
Multiplications  :  1311 


********************************* 


********* i 


